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