Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / ConstraintSet.cpp
1 /* $Id: ConstraintSet.cpp 19905 2009-04-23 13:29:54Z ben2610 $
2  * ConstraintSet.cpp
3  *
4  *  Created on: Jan 5, 2009
5  *      Author: rubensmits
6  */
7
8 #include "ConstraintSet.hpp"
9 #include "kdl/utilities/svd_eigen_HH.hpp"
10
11 namespace iTaSC {
12
13 ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations):
14     m_nc(_nc),
15     m_Jf(e_identity_matrix(6,6)),
16     m_Cf(e_zero_matrix(m_nc,6)),
17     m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)),
18     m_Jf_inv(e_zero_matrix(6,6)),
19     m_Wy(e_scalar_vector(m_nc,1.0)),
20     m_chi(e_zero_vector(6)),m_y(m_nc),m_ydot(e_zero_vector(m_nc)),
21     m_S(6),m_temp(6),m_tdelta(6),
22         m_internalPose(F_identity), m_externalPose(F_identity),
23         m_constraintCallback(NULL), m_constraintParam(NULL), 
24         m_toggle(false),m_substep(false),
25     m_threshold(accuracy),m_maxIter(maximum_iterations)
26 {
27         m_maxDeltaChi = e_scalar(0.52);
28 }
29
30 ConstraintSet::ConstraintSet():
31     m_nc(0), 
32         m_internalPose(F_identity), m_externalPose(F_identity),
33         m_constraintCallback(NULL), m_constraintParam(NULL), 
34         m_toggle(false),m_substep(false),
35         m_threshold(0.0),m_maxIter(0)
36 {
37         m_maxDeltaChi = e_scalar(0.52);
38 }
39
40 void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)
41 {
42     m_nc = _nc;
43     m_Jf = e_identity_matrix(6,6);
44     m_Cf = e_zero_matrix(m_nc,6);
45     m_U = e_identity_matrix(6,6);
46         m_V = e_identity_matrix(6,6);
47         m_B = e_zero_matrix(6,6);
48     m_Jf_inv = e_zero_matrix(6,6),
49     m_Wy = e_scalar_vector(m_nc,1.0),
50     m_chi = e_zero_vector(6);
51     m_chidot = e_zero_vector(6);
52         m_y = e_zero_vector(m_nc);
53         m_ydot = e_zero_vector(m_nc);
54         m_S = e_zero_vector(6);
55         m_temp = e_zero_vector(6);
56         m_tdelta = e_zero_vector(6);
57     m_threshold = accuracy;
58         m_maxIter = maximum_iterations;
59 }
60
61 ConstraintSet::~ConstraintSet() {
62
63 }
64
65 void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp)
66 {
67     m_chi+=m_chidot*timestamp.realTimestep;
68         m_externalPose = _external_pose;
69
70     //update the internal pose and Jf
71     updateJacobian();
72         //check if loop is already closed, if not update the pose and Jf
73     unsigned int iter=0;
74     while(iter<5&&!closeLoop())
75         iter++;
76 }
77
78 double ConstraintSet::getMaxTimestep(double& timestep)
79 {
80         e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff();
81         if (timestep*maxChidot > m_maxDeltaChi) {
82                 timestep = m_maxDeltaChi/maxChidot;
83         }
84         return timestep;
85 }
86
87 bool ConstraintSet::initialise(Frame& init_pose){
88     m_externalPose=init_pose;
89         // get current Jf
90     updateJacobian();
91
92     unsigned int iter=0;
93     while(iter<m_maxIter&&!closeLoop()){
94         iter++;
95     }
96     if (iter<m_maxIter)
97         return true;
98     else
99         return false;
100 }
101
102 bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep)
103 {
104         ConstraintValues values;
105         ConstraintSingleValue value;
106         values.values = &value;
107         values.number = 0;
108         values.action = action;
109         values.id = id;
110         value.action = action;
111         value.id = id;
112         switch (action) {
113         case ACT_NONE:
114                 return true;
115         case ACT_VALUE:
116                 value.yd = data;
117                 values.number = 1;
118                 break;
119         case ACT_VELOCITY:
120                 value.yddot = data;
121                 values.number = 1;
122                 break;
123         case ACT_TOLERANCE:
124                 values.tolerance = data;
125                 break;
126         case ACT_FEEDBACK:
127                 values.feedback = data;
128                 break;
129         case ACT_ALPHA:
130                 values.alpha = data;
131                 break;
132         default:
133                 assert(action==ACT_NONE);
134         }
135         return setControlParameters(&values, 1, timestep);
136 }
137
138 bool ConstraintSet::closeLoop(){
139     //Invert Jf
140     //TODO: svd_boost_Macie has problems if Jf contains zero-rows
141     //toggle=!toggle;
142     //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
143         int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp);
144     if(ret<0)
145         return false;
146
147         // the reference point and frame of the jacobian is the base frame
148         // m_externalPose-m_internalPose is the twist to extend the end effector
149         // to get the required pose => change the reference point to the base frame
150         Twist twist_delta(diff(m_internalPose,m_externalPose));
151         twist_delta=twist_delta.RefPoint(-m_internalPose.p);
152     for(unsigned int i=0;i<6;i++)
153         m_tdelta(i)=twist_delta(i);
154     //TODO: use damping in constraintset inversion?
155     for(unsigned int i=0;i<6;i++)
156         if(m_S(i)<m_threshold){
157                         m_B.row(i).setConstant(0.0);
158         }else
159             m_B.row(i) = m_U.col(i)/m_S(i);
160
161     m_Jf_inv=(m_V*m_B).lazy();
162
163     m_chi+=(m_Jf_inv*m_tdelta).lazy();
164     updateJacobian();
165         // m_externalPose-m_internalPose in end effector frame
166         // this is just to compare the pose, a different formula would work too
167         return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
168
169 }
170 }