1 /** \file itasc/ConstraintSet.cpp
2 * \ingroup itasc
3 */
4 /*
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
ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations)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
ConstraintSet()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
reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations)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
~ConstraintSet()64 ConstraintSet::~ConstraintSet() {
65
66 }
67
modelUpdate(Frame & _external_pose,const Timestamp & timestamp)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
getMaxTimestep(double & timestep)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
initialise(Frame & init_pose)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
setControlParameter(int id,ConstraintAction action,double data,double timestep)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
closeLoop()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 }
166
167 m_Jf_inv.noalias()=m_V*m_B;
168
169 m_chi.noalias()+=m_Jf_inv*m_tdelta;
170 updateJacobian();
171 // m_externalPose-m_internalPose in end effector frame
172 // this is just to compare the pose, a different formula would work too
173 return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold);
174
175 }
176 }
177