diff options
author | erwincoumans <erwin.coumans@gmail.com> | 2019-06-01 19:43:27 -0700 |
---|---|---|
committer | erwincoumans <erwin.coumans@gmail.com> | 2019-06-01 19:43:27 -0700 |
commit | d7c0c9c0179ce7c3e49c656e4b306487059988c4 (patch) | |
tree | a5bf4f42d9f4e54b0e8fe04f58325e71eb6ab350 | |
parent | 36896db8283056e01af2d90dc831aaf7f5af9da2 (diff) | |
download | bullet3-d7c0c9c0179ce7c3e49c656e4b306487059988c4.tar.gz |
update ExampleBrowser InverseKinematics example to track 4 end effectors at once.
-rw-r--r-- | examples/InverseKinematics/InverseKinematicsExample.cpp | 190 | ||||
-rw-r--r-- | examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 4 |
2 files changed, 142 insertions, 52 deletions
diff --git a/examples/InverseKinematics/InverseKinematicsExample.cpp b/examples/InverseKinematics/InverseKinematicsExample.cpp index f4bd6c187..9fea9ce35 100644 --- a/examples/InverseKinematics/InverseKinematicsExample.cpp +++ b/examples/InverseKinematics/InverseKinematicsExample.cpp @@ -72,18 +72,22 @@ void Reset(Tree& tree, Jacobian* m_ikJacobian) // Update target positions -void UpdateTargets(double T2, Tree& treeY) +void UpdateTargets(double T, Tree& treeY) { - double T = T2 / 5.; - targetaa[0].Set(0.6 * b3Sin(0), 0.6 * b3Cos(0), 0.5 + 0.4 * b3Sin(3 * T)); + 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); + 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); + 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); + 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); + } -// Does a single update (on one kind of tree) +// Does a single update (on one kind of m_ikTree) void DoUpdateStep(double Tstep, Tree& treeY, Jacobian* jacob, int ikMethod) { + B3_PROFILE("IK_DoUpdateStep"); if (SleepCounter == 0) { - T += Tstep; + T += Tstep*0.1; UpdateTargets(T, treeY); } @@ -142,7 +146,7 @@ class InverseKinematicsExample : public CommonExampleInterface Jacobian* m_ikJacobian; b3AlignedObjectArray<int> m_movingInstances; - int m_targetInstance; + b3AlignedObjectArray<int> m_targetInstances; enum { numCubesX = 20, @@ -152,10 +156,9 @@ class InverseKinematicsExample : public CommonExampleInterface public: InverseKinematicsExample(CommonGraphicsApp* app, int option) : m_app(app), - m_ikMethod(option), - m_targetInstance(-1) + m_ikMethod(option) { - m_app->setUpAxis(2); + m_app->setUpAxis(1); { b3Vector3 extents = b3MakeVector3(100, 100, 100); @@ -166,7 +169,7 @@ public: b3Vector4 color0 = b3MakeVector4(0.4, 0.4, 0.4, 1); b3Vector4 color1 = b3MakeVector4(0.6, 0.6, 0.6, 1); - m_app->registerGrid(xres, yres, color0, color1); + //m_app->registerGrid(xres, yres, color0, color1); } ///create some graphics proxy for the tracking target @@ -177,8 +180,11 @@ public: pos[app->getUpAxis()] = 1; b3Quaternion orn(0, 0, 0, 1); b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); - b3Vector3 scaling = b3MakeVector3(.02, .02, .02); - m_targetInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling); + b3Vector3 scaling = b3MakeVector3(.1, .1, .1); + m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling)); + m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling)); + m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling)); + m_targetInstances.push_back(m_app->m_renderer->registerGraphicsInstance(sphereId, pos, orn, color, scaling)); } m_app->m_renderer->writeTransforms(); } @@ -215,17 +221,17 @@ public: act.setRotation(rot); act.setOrigin(b3MakeVector3(node->r.x, node->r.y, node->r.z)); } - void MyDrawTree(Node* node, const b3Transform& tr) + void MyDrawTree(Node* node, const b3Transform& tr, const b3Transform& parentTr) { - b3Vector3 lineColor = b3MakeVector3(0, 0, 0); + int lineWidth = 2; if (node != 0) { // glPushMatrix(); b3Vector3 pos = b3MakeVector3(tr.getOrigin().x, tr.getOrigin().y, tr.getOrigin().z); - b3Vector3 color = b3MakeVector3(0, 1, 0); + b3Vector3 color1 = b3MakeVector3(0, 1, 0); int pointSize = 10; - m_app->m_renderer->drawPoint(pos, color, pointSize); + m_app->m_renderer->drawPoint(pos, color1, pointSize); m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(0), b3MakeVector3(1, 0, 0), lineWidth); m_app->m_renderer->drawLine(pos, pos + 0.05 * tr.getBasis().getColumn(1), b3MakeVector3(0, 1, 0), lineWidth); @@ -236,25 +242,29 @@ public: m_app->m_renderer->drawLine(pos, pos + 0.1 * axisWorld, b3MakeVector3(.2, 0.2, 0.7), 5); + // glPopMatrix(); + if (node->right) + { + b3Transform act; + getLocalTransform(node->right, act); + b3Transform trr = tr * act; + b3Transform ptrr = parentTr * act; + b3Vector3 lineColor = b3MakeVector3(0, 1, 0); + m_app->m_renderer->drawLine(tr.getOrigin(), ptrr.getOrigin(), lineColor, lineWidth); + MyDrawTree(node->right, ptrr, parentTr); // Draw right siblings recursively + } + //node->DrawNode(node == root); // Recursively draw node and update ModelView matrix if (node->left) { b3Transform act; getLocalTransform(node->left, act); - + b3Vector3 lineColor = b3MakeVector3(1, 0, 0); b3Transform trl = tr * act; m_app->m_renderer->drawLine(tr.getOrigin(), trl.getOrigin(), lineColor, lineWidth); - MyDrawTree(node->left, trl); // Draw tree of children recursively - } - // glPopMatrix(); - if (node->right) - { - b3Transform act; - getLocalTransform(node->right, act); - b3Transform trr = tr * act; - m_app->m_renderer->drawLine(tr.getOrigin(), trr.getOrigin(), lineColor, lineWidth); - MyDrawTree(node->right, trr); // Draw right siblings recursively + MyDrawTree(node->left, trl, tr); // Draw m_ikTree of children recursively } + } } virtual void stepSimulation(float deltaTime) @@ -265,12 +275,15 @@ public: { b3Transform act; getLocalTransform(m_ikTree.GetRoot(), act); - MyDrawTree(m_ikTree.GetRoot(), act); + MyDrawTree(m_ikTree.GetRoot(), act, b3Transform::getIdentity()); - b3Vector3 pos = b3MakeVector3(targetaa[0].x, targetaa[0].y, targetaa[0].z); - b3Quaternion orn(0, 0, 0, 1); + for (int i = 0; i < m_targetInstances.size(); i++) + { + b3Vector3 pos = b3MakeVector3(targetaa[i].x, targetaa[i].y, targetaa[i].z); + b3Quaternion orn(0, 0, 0, 1); - m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstance); + m_app->m_renderer->writeSingleInstanceTransformToCPU(pos, orn, m_targetInstances[i]); + } m_app->m_renderer->writeTransforms(); m_app->m_renderer->renderScene(); } @@ -309,40 +322,117 @@ public: void InverseKinematicsExample::BuildKukaIIWAShape() { - //const VectorR3& unitx = VectorR3::UnitX; + m_ikNodes.resize(29); + const VectorR3& unitx = VectorR3::UnitX; const VectorR3& unity = VectorR3::UnitY; const VectorR3& unitz = VectorR3::UnitZ; const VectorR3 unit1(sqrt(14.0) / 8.0, 1.0 / 8.0, 7.0 / 8.0); const VectorR3& zero = VectorR3::Zero; - - float minTheta = -4 * PI; - float maxTheta = 4 * PI; - - m_ikNodes.resize(8); //7DOF+additional endeffector - - m_ikNodes[0] = new Node(VectorR3(0.100000, 0.000000, 0.087500), unitz, 0.08, JOINT, -1e30, 1e30, RADIAN(0.)); + VectorR3 p0(0.0f, -1.5f, 0.0f); + VectorR3 p1(0.0f, -1.0f, 0.0f); + VectorR3 p2(0.0f, -0.5f, 0.0f); + VectorR3 p3(0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f); + VectorR3 p4(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f); + VectorR3 p5(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f); + VectorR3 p6(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f); + VectorR3 p7(0.5f*Root2Inv + 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f); + VectorR3 p8(0.5f*Root2Inv + 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f); + VectorR3 p9(0.5f*Root2Inv + 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f); + VectorR3 p10(-0.5f*Root2Inv, -0.5 + 0.5*Root2Inv, 0.0f); + VectorR3 p11(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*HalfRoot3, 0.0f); + VectorR3 p12(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*HalfRoot3, 0.0f); + VectorR3 p13(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*HalfRoot3, 0.0f); + VectorR3 p14(-0.5f*Root2Inv - 0.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 0.5f*0.5, 0.0f); + VectorR3 p15(-0.5f*Root2Inv - 1.0f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.0f*0.5, 0.0f); + VectorR3 p16(-0.5f*Root2Inv - 1.5f*HalfRoot3, -0.5 + 0.5*Root2Inv + 1.5f*0.5, 0.0f); + + m_ikNodes[0] = new Node(p0, unit1, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); m_ikTree.InsertRoot(m_ikNodes[0]); - m_ikNodes[1] = new Node(VectorR3(0.100000, -0.000000, 0.290000), unity, 0.08, JOINT, -0.5, 0.4, RADIAN(0.)); + m_ikNodes[1] = new Node(p1, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); m_ikTree.InsertLeftChild(m_ikNodes[0], m_ikNodes[1]); - m_ikNodes[2] = new Node(VectorR3(0.100000, -0.000000, 0.494500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikNodes[2] = new Node(p1, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); m_ikTree.InsertLeftChild(m_ikNodes[1], m_ikNodes[2]); - m_ikNodes[3] = new Node(VectorR3(0.100000, 0.000000, 0.710000), -unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); + m_ikNodes[3] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); m_ikTree.InsertLeftChild(m_ikNodes[2], m_ikNodes[3]); - m_ikNodes[4] = new Node(VectorR3(0.100000, 0.000000, 0.894500), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); - m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[4]); + m_ikNodes[4] = new Node(p2, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertRightSibling(m_ikNodes[3], m_ikNodes[4]); + + m_ikNodes[5] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[3], m_ikNodes[5]); + + m_ikNodes[6] = new Node(p3, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertRightSibling(m_ikNodes[5], m_ikNodes[6]); + + m_ikNodes[7] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[7]); + + m_ikNodes[8] = new Node(p4, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[7], m_ikNodes[8]); + + m_ikNodes[9] = new Node(p5, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[8], m_ikNodes[9]); + + m_ikNodes[10] = new Node(p5, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[9], m_ikNodes[10]); + + m_ikNodes[11] = new Node(p6, zero, 0.08, EFFECTOR); + m_ikTree.InsertLeftChild(m_ikNodes[10], m_ikNodes[11]); + + m_ikNodes[12] = new Node(p3, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[12]); + + m_ikNodes[13] = new Node(p7, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[12], m_ikNodes[13]); + + m_ikNodes[14] = new Node(p8, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[13], m_ikNodes[14]); + + m_ikNodes[15] = new Node(p8, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[14], m_ikNodes[15]); + + m_ikNodes[16] = new Node(p9, zero, 0.08, EFFECTOR); + m_ikTree.InsertLeftChild(m_ikNodes[15], m_ikNodes[16]); + + m_ikNodes[17] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[17]); + + m_ikNodes[18] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[17], m_ikNodes[18]); + + m_ikNodes[19] = new Node(p10, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertRightSibling(m_ikNodes[17], m_ikNodes[19]); + + m_ikNodes[20] = new Node(p11, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[18], m_ikNodes[20]); + + m_ikNodes[21] = new Node(p12, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[20], m_ikNodes[21]); + + m_ikNodes[22] = new Node(p12, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[21], m_ikNodes[22]); + + m_ikNodes[23] = new Node(p13, zero, 0.08, EFFECTOR); + m_ikTree.InsertLeftChild(m_ikNodes[22], m_ikNodes[23]); + + m_ikNodes[24] = new Node(p10, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[19], m_ikNodes[24]); + + m_ikNodes[25] = new Node(p14, unitz, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[24], m_ikNodes[25]); + + m_ikNodes[26] = new Node(p15, unitx, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[25], m_ikNodes[26]); - m_ikNodes[5] = new Node(VectorR3(0.100000, 0.000000, 1.110000), unity, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); - m_ikTree.InsertLeftChild(m_ikNodes[4], m_ikNodes[5]); + m_ikNodes[27] = new Node(p15, unity, 0.08, JOINT, RADIAN(-180.), RADIAN(180.), RADIAN(30.)); + m_ikTree.InsertLeftChild(m_ikNodes[26], m_ikNodes[27]); - m_ikNodes[6] = new Node(VectorR3(0.100000, 0.000000, 1.191000), unitz, 0.08, JOINT, minTheta, maxTheta, RADIAN(0.)); - m_ikTree.InsertLeftChild(m_ikNodes[5], m_ikNodes[6]); + m_ikNodes[28] = new Node(p16, zero, 0.08, EFFECTOR); + m_ikTree.InsertLeftChild(m_ikNodes[27], m_ikNodes[28]); - m_ikNodes[7] = new Node(VectorR3(0.100000, 0.000000, 1.20000), zero, 0.08, EFFECTOR); - m_ikTree.InsertLeftChild(m_ikNodes[6], m_ikNodes[7]); } class CommonExampleInterface* InverseKinematicsExampleCreateFunc(struct CommonExampleOptions& options) diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index bc9a772cb..3dfff6f7b 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -472,8 +472,8 @@ void Jacobian::CalcDeltaThetasSDLS() // Calculate response vector dTheta that is the SDLS solution. // Delta target values are the dS values int nRows = J.GetNumRows(); - // TODO: Modify it to work with multiple end effectors. - int numEndEffectors = 1; + + int numEndEffectors = m_tree->GetNumEffector(); int nCols = J.GetNumColumns(); dTheta.SetZero(); |