From ba899b371dc96b378944fe06900c75e9d9358a3e Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 2 May 2019 16:18:29 -0700 Subject: [PATCH 1/4] Make btMatrixX::printMatrix const --- src/LinearMath/btMatrixX.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/LinearMath/btMatrixX.h b/src/LinearMath/btMatrixX.h index 0fa8cd0d3..388c57c2d 100644 --- a/src/LinearMath/btMatrixX.h +++ b/src/LinearMath/btMatrixX.h @@ -284,7 +284,7 @@ struct btMatrixX } } - void printMatrix(const char* msg) + void printMatrix(const char* msg) const { printf("%s ---------------------\n", msg); for (int i = 0; i < rows(); i++) From 42471040501cf1cbb337677df76637c648429e6b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 31 May 2019 15:04:53 -0700 Subject: [PATCH 2/4] add toes to Mini Cheetah --- .../mini_cheetah/mini_cheetah.urdf | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) diff --git a/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf b/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf index 64c8391f3..57da7c0a4 100644 --- a/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf +++ b/examples/pybullet/gym/pybullet_data/mini_cheetah/mini_cheetah.urdf @@ -100,7 +100,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -181,6 +214,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -262,6 +328,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -343,5 +442,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From f838e22bd91466da5c5d2bc9f1f5e87fd767261d Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 31 May 2019 19:00:38 -0700 Subject: [PATCH 3/4] fix some warnings --- .../btGeneric6DofSpring2Constraint.cpp | 6 +++--- .../btGeneric6DofSpring2Constraint.h | 2 +- .../Featherstone/btMultiBody.cpp | 4 ++-- src/BulletDynamics/Featherstone/btMultiBody.h | 20 +++++++++---------- 4 files changed, 16 insertions(+), 16 deletions(-) diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp index 8f6d05a71..93626f18f 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.cpp @@ -311,9 +311,9 @@ void btGeneric6DofSpring2Constraint::calculateAngleInfo() case RO_XYZ: { //Is this the "line of nodes" calculation choosing planes YZ (B coordinate system) and xy (A coordinate system)? (http://en.wikipedia.org/wiki/Euler_angles) - //The two planes are non-homologous, so this is a Tait–Bryan angle formalism and not a proper Euler + //The two planes are non-homologous, so this is a Tait Bryan angle formalism and not a proper Euler //Extrinsic rotations are equal to the reversed order intrinsic rotations so the above xyz extrinsic rotations (axes are fixed) are the same as the zy'x" intrinsic rotations (axes are refreshed after each rotation) - //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait–Bryan angles) + //that is why xy and YZ planes are chosen (this will describe a zy'x" intrinsic rotation) (see the figure on the left at http://en.wikipedia.org/wiki/Euler_angles under Tait Bryan angles) // x' = Nperp = N.cross(axis2) // y' = N = axis2.cross(axis0) // z' = z @@ -866,7 +866,7 @@ int btGeneric6DofSpring2Constraint::get_limit_motor_info2( // vel + f / m * (rotational ? -1 : 1) // so in theory this should be set here for m_constraintError // (with m_constraintError we set a desired velocity for the affected body(es)) - // however in practice any value is fine as long as it is greater then the "proper" velocity, + // however in practice any value is fine as long as it is greater than the "proper" velocity, // because the m_lowerLimit and the m_upperLimit will determinate the strength of the final pulling force // so it is much simpler (and more robust) just to simply use inf (with the proper sign) // (Even with our best intent the "new" velocity is only an estimation. If we underestimate diff --git a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h index bc3ee6d21..00e24364e 100644 --- a/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h +++ b/src/BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h @@ -294,7 +294,7 @@ protected: bool m_hasStaticBody; int m_flags; - btGeneric6DofSpring2Constraint& operator=(btGeneric6DofSpring2Constraint&) + btGeneric6DofSpring2Constraint& operator=(const btGeneric6DofSpring2Constraint&) { btAssert(0); return *this; diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index d4f8a42c5..3e210d752 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -352,9 +352,9 @@ void btMultiBody::finalizeMultiDof() updateLinksDofOffsets(); } -int btMultiBody::getParent(int i) const +int btMultiBody::getParent(int link_num) const { - return m_links[i].m_parent; + return m_links[link_num].m_parent; } btScalar btMultiBody::getLinkMass(int i) const diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index 79fae9efd..c0b0d003b 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -65,7 +65,7 @@ public: virtual ~btMultiBody(); //note: fixed link collision with parent is always disabled - void setupFixed(int linkIndex, + void setupFixed(int i, //linkIndex btScalar mass, const btVector3 &inertia, int parent, @@ -83,7 +83,7 @@ public: const btVector3 &thisPivotToThisComOffset, bool disableParentCollision); - void setupRevolute(int linkIndex, // 0 to num_links-1 + void setupRevolute(int i, // 0 to num_links-1 btScalar mass, const btVector3 &inertia, int parentIndex, @@ -93,7 +93,7 @@ public: const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame bool disableParentCollision = false); - void setupSpherical(int linkIndex, // 0 to num_links-1 + void setupSpherical(int i, // linkIndex, 0 to num_links-1 btScalar mass, const btVector3 &inertia, int parent, @@ -277,15 +277,15 @@ public: // // transform vectors in local frame of link i to world frame (or vice versa) // - btVector3 localPosToWorld(int i, const btVector3 &vec) const; - btVector3 localDirToWorld(int i, const btVector3 &vec) const; - btVector3 worldPosToLocal(int i, const btVector3 &vec) const; - btVector3 worldDirToLocal(int i, const btVector3 &vec) const; + btVector3 localPosToWorld(int i, const btVector3 &local_pos) const; + btVector3 localDirToWorld(int i, const btVector3 &local_dir) const; + btVector3 worldPosToLocal(int i, const btVector3 &world_pos) const; + btVector3 worldDirToLocal(int i, const btVector3 &world_dir) const; // // transform a frame in local coordinate to a frame in world coordinate // - btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const; + btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &local_frame) const; // // calculate kinetic energy and angular momentum @@ -576,11 +576,11 @@ public: { return m_internalNeedsJointFeedback; } - void forwardKinematics(btAlignedObjectArray & scratch_q, btAlignedObjectArray & scratch_m); + void forwardKinematics(btAlignedObjectArray& world_to_local, btAlignedObjectArray & local_origin); void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const; - void updateCollisionObjectWorldTransforms(btAlignedObjectArray & scratch_q, btAlignedObjectArray & scratch_m); + void updateCollisionObjectWorldTransforms(btAlignedObjectArray & world_to_local, btAlignedObjectArray & local_origin); virtual int calculateSerializeBufferSize() const; From d7c0c9c0179ce7c3e49c656e4b306487059988c4 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Sat, 1 Jun 2019 19:43:27 -0700 Subject: [PATCH 4/4] update ExampleBrowser InverseKinematics example to track 4 end effectors at once. --- .../InverseKinematicsExample.cpp | 190 +++++++++++++----- 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 m_movingInstances; - int m_targetInstance; + b3AlignedObjectArray 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); - //node->DrawNode(node == root); // Recursively draw node and update ModelView matrix - if (node->left) - { - b3Transform act; - getLocalTransform(node->left, act); - - 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 + 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, 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; + 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); - 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.)); + 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(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[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(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[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[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[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();