1 #include "IKTrajectoryHelper.h"
2 #include "BussIK/Node.h"
3 #include "BussIK/Tree.h"
4 #include "BussIK/Jacobian.h"
5 #include "BussIK/VectorRn.h"
6 #include "BussIK/MatrixRmn.h"
7 #include "Bullet3Common/b3AlignedObjectArray.h"
8 #include "BulletDynamics/Featherstone/btMultiBody.h"
9
10 #define RADIAN(X) ((X)*RadiansToDegrees)
11
12 //use BussIK and Reflexxes to convert from Cartesian endeffector future target to
13 //joint space positions at each real-time (simulation) step
14 struct IKTrajectoryHelperInternalData
15 {
16 VectorR3 m_endEffectorTargetPosition;
17 VectorRn m_nullSpaceVelocity;
18 VectorRn m_dampingCoeff;
19
20 b3AlignedObjectArray<Node*> m_ikNodes;
21
IKTrajectoryHelperInternalDataIKTrajectoryHelperInternalData22 IKTrajectoryHelperInternalData()
23 {
24 m_endEffectorTargetPosition.SetZero();
25 m_nullSpaceVelocity.SetZero();
26 m_dampingCoeff.SetZero();
27 }
28 };
29
IKTrajectoryHelper()30 IKTrajectoryHelper::IKTrajectoryHelper()
31 {
32 m_data = new IKTrajectoryHelperInternalData;
33 }
34
~IKTrajectoryHelper()35 IKTrajectoryHelper::~IKTrajectoryHelper()
36 {
37 delete m_data;
38 }
39
computeIK(const double endEffectorTargetPosition[3],const double endEffectorTargetOrientation[4],const double endEffectorWorldPosition[3],const double endEffectorWorldOrientation[4],const double * q_current,int numQ,int endEffectorIndex,double * q_new,int ikMethod,const double * linear_jacobian,const double * angular_jacobian,int jacobian_size,const double dampIk[6])40 bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
41 const double endEffectorTargetOrientation[4],
42 const double endEffectorWorldPosition[3],
43 const double endEffectorWorldOrientation[4],
44 const double* q_current, int numQ, int endEffectorIndex,
45 double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
46 {
47 MatrixRmn AugMat;
48 bool useAngularPart = (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
49
50 Jacobian ikJacobian(useAngularPart, numQ, 1);
51
52 ikJacobian.Reset();
53
54 bool UseJacobianTargets1 = false;
55
56 if (UseJacobianTargets1)
57 {
58 ikJacobian.SetJtargetActive();
59 }
60 else
61 {
62 ikJacobian.SetJendActive();
63 }
64 VectorR3 targets;
65 targets.Set(endEffectorTargetPosition[0], endEffectorTargetPosition[1], endEffectorTargetPosition[2]);
66 ikJacobian.ComputeJacobian(&targets); // Set up Jacobian and deltaS vectors
67
68 // Set one end effector world position from Bullet
69 VectorRn deltaS(3);
70 for (int i = 0; i < 3; ++i)
71 {
72 deltaS.Set(i, dampIk[i] * (endEffectorTargetPosition[i] - endEffectorWorldPosition[i]));
73 }
74
75 // Set one end effector world orientation from Bullet
76 VectorRn deltaR(3);
77 if (useAngularPart)
78 {
79 btQuaternion startQ(endEffectorWorldOrientation[0], endEffectorWorldOrientation[1], endEffectorWorldOrientation[2], endEffectorWorldOrientation[3]);
80 btQuaternion endQ(endEffectorTargetOrientation[0], endEffectorTargetOrientation[1], endEffectorTargetOrientation[2], endEffectorTargetOrientation[3]);
81 btQuaternion deltaQ = endQ * startQ.inverse();
82 float angle = deltaQ.getAngle();
83 btVector3 axis = deltaQ.getAxis();
84 if (angle > PI)
85 {
86 angle -= 2.0 * PI;
87 }
88 else if (angle < -PI)
89 {
90 angle += 2.0 * PI;
91 }
92 float angleDot = angle;
93 btVector3 angularVel = angleDot * axis.normalize();
94 for (int i = 0; i < 3; ++i)
95 {
96 deltaR.Set(i, dampIk[i + 3] * angularVel[i]);
97 }
98 }
99
100 {
101 if (useAngularPart)
102 {
103 VectorRn deltaC(6);
104 MatrixRmn completeJacobian(6, numQ);
105 for (int i = 0; i < 3; ++i)
106 {
107 deltaC.Set(i, deltaS[i]);
108 deltaC.Set(i + 3, deltaR[i]);
109 for (int j = 0; j < numQ; ++j)
110 {
111 completeJacobian.Set(i, j, linear_jacobian[i * numQ + j]);
112 completeJacobian.Set(i + 3, j, angular_jacobian[i * numQ + j]);
113 }
114 }
115 ikJacobian.SetDeltaS(deltaC);
116 ikJacobian.SetJendTrans(completeJacobian);
117 }
118 else
119 {
120 VectorRn deltaC(3);
121 MatrixRmn completeJacobian(3, numQ);
122 for (int i = 0; i < 3; ++i)
123 {
124 deltaC.Set(i, deltaS[i]);
125 for (int j = 0; j < numQ; ++j)
126 {
127 completeJacobian.Set(i, j, linear_jacobian[i * numQ + j]);
128 }
129 }
130 ikJacobian.SetDeltaS(deltaC);
131 ikJacobian.SetJendTrans(completeJacobian);
132 }
133 }
134
135 // Calculate the change in theta values
136 switch (ikMethod)
137 {
138 case IK2_JACOB_TRANS:
139 ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
140 break;
141 case IK2_DLS:
142 case IK2_VEL_DLS_WITH_ORIENTATION:
143 case IK2_VEL_DLS:
144 //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
145 assert(m_data->m_dampingCoeff.GetLength() == numQ);
146 ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
147 break;
148 case IK2_VEL_DLS_WITH_NULLSPACE:
149 case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
150 assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
151 ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
152 break;
153 case IK2_DLS_SVD:
154 ikJacobian.CalcDeltaThetasDLSwithSVD();
155 break;
156 case IK2_PURE_PSEUDO:
157 ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
158 break;
159 case IK2_SDLS:
160 case IK2_VEL_SDLS:
161 case IK2_VEL_SDLS_WITH_ORIENTATION:
162 ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
163 break;
164 default:
165 ikJacobian.ZeroDeltaThetas();
166 break;
167 }
168
169 // Use for velocity IK, update theta dot
170 //ikJacobian.UpdateThetaDot();
171
172 // Use for position IK, incrementally update theta
173 //ikJacobian.UpdateThetas();
174
175 // Apply the change in the theta values
176 //ikJacobian.UpdatedSClampValue(&targets);
177
178 for (int i = 0; i < numQ; i++)
179 {
180 // Use for velocity IK
181 q_new[i] = ikJacobian.dTheta[i] + q_current[i];
182
183 // Use for position IK
184 //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
185 }
186 return true;
187 }
188
189
computeIK2(const double * endEffectorTargetPositions,const double * endEffectorCurrentPositions,int numEndEffectors,const double * q_current,int numQ,double * q_new,int ikMethod,const double * linear_jacobians,const double dampIk[6])190 bool IKTrajectoryHelper::computeIK2(
191 const double* endEffectorTargetPositions,
192 const double* endEffectorCurrentPositions,
193 int numEndEffectors,
194 const double* q_current, int numQ,
195 double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6])
196 {
197 MatrixRmn AugMat;
198 bool useAngularPart = false;//for now (ikMethod == IK2_VEL_DLS_WITH_ORIENTATION || ikMethod == IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE || ikMethod == IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
199
200 Jacobian ikJacobian(useAngularPart, numQ, numEndEffectors);
201
202 ikJacobian.Reset();
203
204 bool UseJacobianTargets1 = false;
205
206 if (UseJacobianTargets1)
207 {
208 ikJacobian.SetJtargetActive();
209 }
210 else
211 {
212 ikJacobian.SetJendActive();
213 }
214
215 VectorRn deltaC(numEndEffectors *3);
216 MatrixRmn completeJacobian(numEndEffectors*3, numQ);
217
218 for (int ne = 0; ne < numEndEffectors; ne++)
219 {
220 VectorR3 targets;
221 targets.Set(endEffectorTargetPositions[ne*3+0], endEffectorTargetPositions[ne * 3 + 1], endEffectorTargetPositions[ne * 3 + 2]);
222
223 // Set one end effector world position from Bullet
224 VectorRn deltaS(3);
225 for (int i = 0; i < 3; ++i)
226 {
227 deltaS.Set(i, dampIk[i] * (endEffectorTargetPositions[ne*3+i] - endEffectorCurrentPositions[ne*3+i]));
228 }
229 {
230
231 for (int i = 0; i < 3; ++i)
232 {
233 deltaC.Set(ne*3+i, deltaS[i]);
234 for (int j = 0; j < numQ; ++j)
235 {
236 completeJacobian.Set(ne * 3 + i, j, linear_jacobians[((ne*3+i) * numQ) + j]);
237 }
238 }
239 }
240 }
241 ikJacobian.SetDeltaS(deltaC);
242 ikJacobian.SetJendTrans(completeJacobian);
243
244 // Calculate the change in theta values
245 switch (ikMethod)
246 {
247 case IK2_JACOB_TRANS:
248 ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
249 break;
250 case IK2_DLS:
251 case IK2_VEL_DLS_WITH_ORIENTATION:
252 case IK2_VEL_DLS:
253 //ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
254 assert(m_data->m_dampingCoeff.GetLength() == numQ);
255 ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff, AugMat);
256 break;
257 case IK2_VEL_DLS_WITH_NULLSPACE:
258 case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE:
259 assert(m_data->m_nullSpaceVelocity.GetLength() == numQ);
260 ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity, AugMat);
261 break;
262 case IK2_DLS_SVD:
263 ikJacobian.CalcDeltaThetasDLSwithSVD();
264 break;
265 case IK2_PURE_PSEUDO:
266 ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
267 break;
268 case IK2_SDLS:
269 case IK2_VEL_SDLS:
270 case IK2_VEL_SDLS_WITH_ORIENTATION:
271 ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
272 break;
273 default:
274 ikJacobian.ZeroDeltaThetas();
275 break;
276 }
277
278 // Use for velocity IK, update theta dot
279 //ikJacobian.UpdateThetaDot();
280
281 // Use for position IK, incrementally update theta
282 //ikJacobian.UpdateThetas();
283
284 // Apply the change in the theta values
285 //ikJacobian.UpdatedSClampValue(&targets);
286
287 for (int i = 0; i < numQ; i++)
288 {
289 // Use for velocity IK
290 q_new[i] = ikJacobian.dTheta[i] + q_current[i];
291
292 // Use for position IK
293 //q_new[i] = m_data->m_ikNodes[i]->GetTheta();
294 }
295 return true;
296 }
297
298
computeNullspaceVel(int numQ,const double * q_current,const double * lower_limit,const double * upper_limit,const double * joint_range,const double * rest_pose)299 bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose)
300 {
301 m_data->m_nullSpaceVelocity.SetLength(numQ);
302 m_data->m_nullSpaceVelocity.SetZero();
303 // TODO: Expose the coefficents of the null space term so that the user can choose to balance the null space task and the IK target task.
304 // Can also adaptively adjust the coefficients based on the residual of the null space velocity in the IK target task space.
305 double stayCloseToZeroGain = 0.001;
306 double stayAwayFromLimitsGain = 10.0;
307
308 // Stay close to zero
309 for (int i = 0; i < numQ; ++i)
310 {
311 m_data->m_nullSpaceVelocity[i] = stayCloseToZeroGain * (rest_pose[i] - q_current[i]);
312 }
313
314 // Stay away from joint limits
315 for (int i = 0; i < numQ; ++i)
316 {
317 if (q_current[i] > upper_limit[i])
318 {
319 m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (upper_limit[i] - q_current[i]) / joint_range[i];
320 }
321 if (q_current[i] < lower_limit[i])
322 {
323 m_data->m_nullSpaceVelocity[i] += stayAwayFromLimitsGain * (lower_limit[i] - q_current[i]) / joint_range[i];
324 }
325 }
326 return true;
327 }
328
setDampingCoeff(int numQ,const double * coeff)329 bool IKTrajectoryHelper::setDampingCoeff(int numQ, const double* coeff)
330 {
331 m_data->m_dampingCoeff.SetLength(numQ);
332 m_data->m_dampingCoeff.SetZero();
333 for (int i = 0; i < numQ; ++i)
334 {
335 m_data->m_dampingCoeff[i] = coeff[i];
336 }
337 return true;
338 }
339