1 #include "InverseKinematicsExample.h"
2
3 #include "../CommonInterfaces/CommonGraphicsAppInterface.h"
4 #include "Bullet3Common/b3Quaternion.h"
5 #include "Bullet3Common/b3Transform.h"
6 #include "Bullet3Common/b3AlignedObjectArray.h"
7 #include "../CommonInterfaces/CommonRenderInterface.h"
8 #include "../CommonInterfaces/CommonExampleInterface.h"
9 #include "../CommonInterfaces/CommonGUIHelperInterface.h"
10
11 #include "BussIK/Node.h"
12 #include "BussIK/Tree.h"
13 #include "BussIK/Jacobian.h"
14 #include "BussIK/VectorRn.h"
15
16 #define RADIAN(X) ((X)*RadiansToDegrees)
17
18 #define MAX_NUM_NODE 1000
19 #define MAX_NUM_THETA 1000
20 #define MAX_NUM_EFFECT 100
21
22 double T = 0;
23 VectorR3 targetaa[MAX_NUM_EFFECT];
24
25 // Make slowdown factor larger to make the simulation take larger, less frequent steps
26 // Make the constant factor in Tstep larger to make time pass more quickly
27 //const int SlowdownFactor = 40;
28 const int SlowdownFactor = 0; // Make higher to take larger steps less frequently
29 const int SleepsPerStep = SlowdownFactor;
30 int SleepCounter = 0;
31 //const double Tstep = 0.0005*(double)SlowdownFactor; // Time step
32
33 int AxesList; /* list to hold the axes */
34 int AxesOn; /* ON or OFF */
35
36 float Scale, Scale2; /* scaling factors */
37
38 int JointLimitsOn;
39 int RestPositionOn;
40 int UseJacobianTargets1;
41
42 int numIteration = 1;
43 double error = 0.0;
44 double errorDLS = 0.0;
45 double errorSDLS = 0.0;
46 double sumError = 0.0;
47 double sumErrorDLS = 0.0;
48 double sumErrorSDLS = 0.0;
49
50 #ifdef _DYNAMIC
51 bool initMaxDist = true;
52 extern double Excess[];
53 extern double dsnorm[];
54 #endif
55
Reset(Tree & tree,Jacobian * m_ikJacobian)56 void Reset(Tree& tree, Jacobian* m_ikJacobian)
57 {
58 AxesOn = false;
59
60 Scale = 1.0;
61 Scale2 = 0.0; /* because add 1. to it in Display() */
62
63 JointLimitsOn = true;
64 RestPositionOn = false;
65 UseJacobianTargets1 = false;
66
67 tree.Init();
68 tree.Compute();
69 m_ikJacobian->Reset();
70 }
71
72 // Update target positions
73
UpdateTargets(double T,Tree & treeY)74 void UpdateTargets(double T, Tree& treeY)
75 {
76 targetaa[0].Set(2.0f + 1.5*sin(3 * T) * 2, -0.5 + 1.0f + 0.2*sin(7 * T) * 2, 0.3f + 0.7*sin(5 * T) * 2);
77 targetaa[1].Set(0.5f + 0.4*sin(4 * T) * 2, -0.5 + 0.9f + 0.3*sin(4 * T) * 2, -0.2f + 1.0*sin(3 * T) * 2);
78 targetaa[2].Set(-0.5f + 0.8*sin(6 * T) * 2, -0.5 + 1.1f + 0.2*sin(7 * T) * 2, 0.3f + 0.5*sin(8 * T) * 2);
79 targetaa[3].Set(-1.6f + 0.8*sin(4 * T) * 2, -0.5 + 0.8f + 0.3*sin(4 * T) * 2, -0.2f + 0.3*sin(3 * T) * 2);
80
81 }
82
83 // Does a single update (on one kind of m_ikTree)
DoUpdateStep(double Tstep,Tree & treeY,Jacobian * jacob,int ikMethod)84 void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod)
85 {
86 B3_PROFILE("IK_DoUpdateStep");
87 if (SleepCounter == 0)
88 {
89 T += Tstep*0.1;
90 UpdateTargets(T, treeY);
91 }
92
93 if (UseJacobianTargets1)
94 {
95 jacob->SetJtargetActive();
96 }
97 else
98 {
99 jacob->SetJendActive();
100 }
101 jacob->ComputeJacobian(targetaa); // Set up Jacobian and deltaS vectors
102 MatrixRmn AugMat;
103 // Calculate the change in theta values
104 switch (ikMethod)
105 {
106 case IK_JACOB_TRANS:
107 jacob->CalcDeltaThetasTranspose(); // Jacobian transpose method
108 break;
109 case IK_DLS:
110 jacob->CalcDeltaThetasDLS(AugMat); // Damped least squares method
111 break;
112 case IK_DLS_SVD:
113 jacob->CalcDeltaThetasDLSwithSVD();
114 break;
115 case IK_PURE_PSEUDO:
116 jacob->CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
117 break;
118 case IK_SDLS:
119 jacob->CalcDeltaThetasSDLS(); // Selectively damped least squares method
120 break;
121 default:
122 jacob->ZeroDeltaThetas();
123 break;
124 }
125
126 if (SleepCounter == 0)
127 {
128 jacob->UpdateThetas(); // Apply the change in the theta values
129 jacob->UpdatedSClampValue(targetaa);
130 SleepCounter = SleepsPerStep;
131 }
132 else
133 {
134 SleepCounter--;
135 }
136 }
137
138 ///quick demo showing the right-handed coordinate system and positive rotations around each axis
139 class InverseKinematicsExample : public CommonExampleInterface
140 {
141 CommonGraphicsApp* m_app;
142 int m_ikMethod;
143 Tree m_ikTree;
144 b3AlignedObjectArray<Node*> m_ikNodes;
145 Jacobian* m_ikJacobian;
146
147 b3AlignedObjectArray<int> m_movingInstances;
148 b3AlignedObjectArray<int> m_targetInstances;
149 enum
150 {
151 numCubesX = 20,
152 numCubesY = 20
153 };
154
155 public:
InverseKinematicsExample(CommonGraphicsApp * app,int option)156 InverseKinematicsExample(CommonGraphicsApp* app, int option)
157 : m_app(app),
158 m_ikMethod(option)
159 {
160 m_app->setUpAxis(1);
161
162 {
163 b3Vector3 extents = b3MakeVector3(100, 100, 100);
164 extents[m_app->getUpAxis()] = 1;
165
166 int xres = 20;
167 int yres = 20;
168
169 b3Vector4 color0 = b3MakeVector4(0.4, 0.4, 0.4, 1);
170 b3Vector4 color1 = b3MakeVector4(0.6, 0.6, 0.6, 1);
171 //m_app->registerGrid(xres, yres, color0, color1);
172 }
173
174 ///create some graphics proxy for the tracking target
175 ///the endeffector tries to track it using Inverse Kinematics
176 {
177 int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
178 b3Vector3 pos = b3MakeVector3(1, 1, 1);
179 pos[app->getUpAxis()] = 1;
180 b3Quaternion orn(0, 0, 0, 1);
181 b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
182 b3Vector3 scaling = b3MakeVector3(.1, .1, .1);
183 m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
184 m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
185 m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
186 m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling));
187 }
188 m_app->m_renderer->writeTransforms();
189 }
~InverseKinematicsExample()190 virtual ~InverseKinematicsExample()
191 {
192 }
193
physicsDebugDraw(int debugDrawMode)194 virtual void physicsDebugDraw(int debugDrawMode)
195 {
196 }
initPhysics()197 virtual void initPhysics()
198 {
199 BuildKukaIIWAShape();
200 m_ikJacobian = new Jacobian(&m_ikTree);
201 Reset(m_ikTree, m_ikJacobian);
202 }
exitPhysics()203 virtual void exitPhysics()
204 {
205 delete m_ikJacobian;
206 m_ikJacobian = 0;
207 }
208
209 void BuildKukaIIWAShape();
210
getLocalTransform(const Node * node,b3Transform & act)211 void getLocalTransform(const Node* node, b3Transform& act)
212 {
213 b3Vector3 axis = b3MakeVector3(node->v.x, node->v.y, node->v.z);
214 b3Quaternion rot(0, 0, 0, 1);
215 if (axis.length())
216 {
217 rot = b3Quaternion(axis, node->theta);
218 }
219 act.setIdentity();
220 act.setRotation(rot);
221 act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z));
222 }
MyDrawTree(Node * node,const b3Transform & tr,const b3Transform & parentTr)223 void MyDrawTree(Node* node, const b3Transform& tr, const b3Transform& parentTr)
224 {
225
226 int lineWidth = 2;
227 if (node != 0)
228 {
229 b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z);
230 b3Vector3 color1 = b3MakeVector3(0, 1, 0);
231 int pointSize = 10;
232 m_app->m_renderer->drawPoint(pos, color1, pointSize);
233
234 m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(0), b3MakeVector3(1, 0, 0), lineWidth);
235 m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth);
236 m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(2), b3MakeVector3(0, 0, 1), lineWidth);
237
238 b3Vector3 axisLocal = b3MakeVector3(node->v.x, node->v.y, node->v.z);
239 b3Vector3 axisWorld = tr.getBasis() * axisLocal;
240
241 m_app->m_renderer->drawLine(pos, pos + 0.1 * axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5);
242
243 if (node->right)
244 {
245 b3Transform act;
246 getLocalTransform(node->right, act);
247 b3Transform trr = tr * act;
248 b3Transform ptrr = parentTr * act;
249 b3Vector3 lineColor = b3MakeVector3(0, 1, 0);
250 m_app->m_renderer->drawLine(tr.getOrigin(), ptrr.getOrigin(), lineColor, lineWidth);
251 MyDrawTree(node->right, ptrr, parentTr); // Draw right siblings recursively
252 }
253
254 //node->DrawNode(node == root); // Recursively draw node and update ModelView matrix
255 if (node->left)
256 {
257 b3Transform act;
258 getLocalTransform(node->left, act);
259 b3Vector3 lineColor = b3MakeVector3(1, 0, 0);
260 b3Transform trl = tr * act;
261 m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth);
262 MyDrawTree(node->left, trl, tr); // Draw m_ikTree of children recursively
263 }
264
265 }
266 }
stepSimulation(float deltaTime)267 virtual void stepSimulation(float deltaTime)
268 {
269 DoUpdateStep(deltaTime, m_ikTree, m_ikJacobian, m_ikMethod);
270 }
renderScene()271 virtual void renderScene()
272 {
273 b3Transform act;
274 getLocalTransform(m_ikTree.GetRoot(), act);
275 MyDrawTree(m_ikTree.GetRoot(), act, b3Transform::getIdentity());
276
277 for (int i = 0; i < m_targetInstances.size(); i++)
278 {
279 b3Vector3 pos = b3MakeVector3(targetaa[i].x, targetaa[i].y, targetaa[i].z);
280 b3Quaternion orn(0, 0, 0, 1);
281
282 m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstances[i]);
283 }
284 m_app->m_renderer->writeTransforms();
285 m_app->m_renderer->renderScene();
286 }
287
physicsDebugDraw()288 virtual void physicsDebugDraw()
289 {
290 }
mouseMoveCallback(float x,float y)291 virtual bool mouseMoveCallback(float x, float y)
292 {
293 return false;
294 }
mouseButtonCallback(int button,int state,float x,float y)295 virtual bool mouseButtonCallback(int button, int state, float x, float y)
296 {
297 return false;
298 }
keyboardCallback(int key,int state)299 virtual bool keyboardCallback(int key, int state)
300 {
301 return false;
302 }
303
resetCamera()304 virtual void resetCamera()
305 {
306 float dist = 1.3;
307 float pitch = -13;
308 float yaw = 120;
309 float targetPos[3] = {-0.35, 0.14, 0.25};
310 if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
311 {
312 m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
313 m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
314 m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
315 m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
316 }
317 }
318 };
319
BuildKukaIIWAShape()320 void InverseKinematicsExample::BuildKukaIIWAShape()
321 {
322 m_ikNodes.resize(29);
323 const VectorR3& unitx = VectorR3::UnitX;
324 const VectorR3& unity = VectorR3::UnitY;
325 const VectorR3& unitz = VectorR3::UnitZ;
326 const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0);
327 const VectorR3& zero = VectorR3::Zero;
328 VectorR3 p0(0.0f, -1.5f, 0.0f);
329 VectorR3 p1(0.0f, -1.0f, 0.0f);
330 VectorR3 p2(0.0f, -0.5f, 0.0f);
331 VectorR3 p3(0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
332 VectorR3 p4(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
333 VectorR3 p5(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
334 VectorR3 p6(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
335 VectorR3 p7(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
336 VectorR3 p8(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
337 VectorR3 p9(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
338 VectorR3 p10(-0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f);
339 VectorR3 p11(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f);
340 VectorR3 p12(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f);
341 VectorR3 p13(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f);
342 VectorR3 p14(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f);
343 VectorR3 p15(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f);
344 VectorR3 p16(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f);
345
346 m_ikNodes[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
347 m_ikTree.InsertRoot(m_ikNodes[0]);
348
349 m_ikNodes[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
350 m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]);
351
352 m_ikNodes[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
353 m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]);
354
355 m_ikNodes[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
356 m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]);
357
358 m_ikNodes[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
359 m_ikTree.InsertRightSibling(m_ikNodes[3], m_ikNodes[4]);
360
361 m_ikNodes[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
362 m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[5]);
363
364 m_ikNodes[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
365 m_ikTree.InsertRightSibling(m_ikNodes[5], m_ikNodes[6]);
366
367 m_ikNodes[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
368 m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[7]);
369
370 m_ikNodes[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
371 m_ikTree.InsertLeftChild(m_ikNodes[7], m_ikNodes[8]);
372
373 m_ikNodes[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
374 m_ikTree.InsertLeftChild(m_ikNodes[8], m_ikNodes[9]);
375
376 m_ikNodes[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
377 m_ikTree.InsertLeftChild(m_ikNodes[9], m_ikNodes[10]);
378
379 m_ikNodes[11] = new Node(p6, zero, 0.08, EFFECTOR);
380 m_ikTree.InsertLeftChild(m_ikNodes[10], m_ikNodes[11]);
381
382 m_ikNodes[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
383 m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[12]);
384
385 m_ikNodes[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
386 m_ikTree.InsertLeftChild(m_ikNodes[12], m_ikNodes[13]);
387
388 m_ikNodes[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
389 m_ikTree.InsertLeftChild(m_ikNodes[13], m_ikNodes[14]);
390
391 m_ikNodes[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
392 m_ikTree.InsertLeftChild(m_ikNodes[14], m_ikNodes[15]);
393
394 m_ikNodes[16] = new Node(p9, zero, 0.08, EFFECTOR);
395 m_ikTree.InsertLeftChild(m_ikNodes[15], m_ikNodes[16]);
396
397 m_ikNodes[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
398 m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[17]);
399
400 m_ikNodes[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
401 m_ikTree.InsertLeftChild(m_ikNodes[17], m_ikNodes[18]);
402
403 m_ikNodes[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
404 m_ikTree.InsertRightSibling(m_ikNodes[17], m_ikNodes[19]);
405
406 m_ikNodes[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
407 m_ikTree.InsertLeftChild(m_ikNodes[18], m_ikNodes[20]);
408
409 m_ikNodes[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
410 m_ikTree.InsertLeftChild(m_ikNodes[20], m_ikNodes[21]);
411
412 m_ikNodes[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
413 m_ikTree.InsertLeftChild(m_ikNodes[21], m_ikNodes[22]);
414
415 m_ikNodes[23] = new Node(p13, zero, 0.08, EFFECTOR);
416 m_ikTree.InsertLeftChild(m_ikNodes[22], m_ikNodes[23]);
417
418 m_ikNodes[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
419 m_ikTree.InsertLeftChild(m_ikNodes[19], m_ikNodes[24]);
420
421 m_ikNodes[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
422 m_ikTree.InsertLeftChild(m_ikNodes[24], m_ikNodes[25]);
423
424 m_ikNodes[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
425 m_ikTree.InsertLeftChild(m_ikNodes[25], m_ikNodes[26]);
426
427 m_ikNodes[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.));
428 m_ikTree.InsertLeftChild(m_ikNodes[26], m_ikNodes[27]);
429
430 m_ikNodes[28] = new Node(p16, zero, 0.08, EFFECTOR);
431 m_ikTree.InsertLeftChild(m_ikNodes[27], m_ikNodes[28]);
432
433 }
434
InverseKinematicsExampleCreateFunc(struct CommonExampleOptions & options)435 class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options)
436 {
437 return new InverseKinematicsExample(options.m_guiHelper->getAppInterface(), options.m_option);
438 }
439