From bf16c8798705a991186a62224ab3d4cec84739d8 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Mon, 19 Sep 2016 17:04:05 -0700 Subject: [PATCH 1/3] Add orientation constraint to IK. --- .../RoboticsLearning/KukaGraspExample.cpp | 34 ++++++----- examples/RoboticsLearning/b3RobotSimAPI.cpp | 9 ++- examples/RoboticsLearning/b3RobotSimAPI.h | 5 +- examples/SharedMemory/IKTrajectoryHelper.cpp | 56 +++++++++++++------ examples/SharedMemory/IKTrajectoryHelper.h | 7 ++- examples/SharedMemory/PhysicsClientC_API.cpp | 9 ++- examples/SharedMemory/PhysicsClientC_API.h | 3 +- .../PhysicsServerCommandProcessor.cpp | 18 ++++-- examples/SharedMemory/SharedMemoryCommands.h | 3 +- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 27 ++++++++- examples/ThirdPartyLibs/BussIK/Jacobian.h | 1 + 11 files changed, 125 insertions(+), 47 deletions(-) diff --git a/examples/RoboticsLearning/KukaGraspExample.cpp b/examples/RoboticsLearning/KukaGraspExample.cpp index b8c2d4eec..f5735d0e0 100644 --- a/examples/RoboticsLearning/KukaGraspExample.cpp +++ b/examples/RoboticsLearning/KukaGraspExample.cpp @@ -27,6 +27,8 @@ class KukaGraspExample : public CommonExampleInterface int m_targetSphereInstance; b3Vector3 m_targetPos; b3Vector3 m_worldPos; + b3Vector4 m_targetOri; + b3Vector4 m_worldOri; double m_time; int m_options; @@ -145,7 +147,7 @@ public: m_time+=dt; m_targetPos.setValue(0.4-0.4*b3Cos( m_time), 0, 0.8+0.4*b3Cos( m_time)); - + m_targetOri.setValue(0, 1.0, 0, 0); int numJoints = m_robotSim.getNumJoints(m_kukaIndex); @@ -155,6 +157,7 @@ public: double q_current[7]={0,0,0,0,0,0,0}; double world_position[3]={0,0,0}; + double world_orientation[4]={0,0,0,0}; b3JointStates jointStates; if (m_robotSim.getJointStates(m_kukaIndex,jointStates)) @@ -166,14 +169,19 @@ public: q_current[i] = jointStates.m_actualStateQ[i+7]; } } - // compute body position - m_robotSim.getLinkState(0, 6, world_position); + // compute body position and orientation + m_robotSim.getLinkState(0, 6, world_position, world_orientation); m_worldPos.setValue(world_position[0], world_position[1], world_position[2]); + m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]); - b3Vector3DoubleData dataOut; - m_targetPos.serializeDouble(dataOut); + b3Vector3DoubleData targetPosDataOut; + m_targetPos.serializeDouble(targetPosDataOut); b3Vector3DoubleData worldPosDataOut; m_worldPos.serializeDouble(worldPosDataOut); + b3Vector3DoubleData targetOriDataOut; + m_targetOri.serializeDouble(targetOriDataOut); + b3Vector3DoubleData worldOriDataOut; + m_worldOri.serializeDouble(worldOriDataOut); b3RobotSimInverseKinematicArgs ikargs; @@ -183,15 +191,15 @@ public: ikargs.m_bodyUniqueId = m_kukaIndex; // ikargs.m_currentJointPositions = q_current; // ikargs.m_numPositions = 7; - ikargs.m_endEffectorTargetPosition[0] = dataOut.m_floats[0]; - ikargs.m_endEffectorTargetPosition[1] = dataOut.m_floats[1]; - ikargs.m_endEffectorTargetPosition[2] = dataOut.m_floats[2]; + ikargs.m_endEffectorTargetPosition[0] = targetPosDataOut.m_floats[0]; + ikargs.m_endEffectorTargetPosition[1] = targetPosDataOut.m_floats[1]; + ikargs.m_endEffectorTargetPosition[2] = targetPosDataOut.m_floats[2]; -//todo: orientation IK target -// ikargs.m_endEffectorTargetOrientation[0] = 0; -// ikargs.m_endEffectorTargetOrientation[1] = 0; -// ikargs.m_endEffectorTargetOrientation[2] = 0; -// ikargs.m_endEffectorTargetOrientation[3] = 1; + ikargs.m_endEffectorTargetOrientation[0] = targetOriDataOut.m_floats[0]; + ikargs.m_endEffectorTargetOrientation[1] = targetOriDataOut.m_floats[1]; + ikargs.m_endEffectorTargetOrientation[2] = targetOriDataOut.m_floats[2]; + ikargs.m_endEffectorTargetOrientation[3] = targetOriDataOut.m_floats[3]; + ikargs.m_dt = dt; if (m_robotSim.calculateInverseKinematics(ikargs,ikresults)) { diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 81a9872a3..873ddfc36 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -481,8 +481,7 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status bool b3RobotSimAPI::calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results) { - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, - args.m_endEffectorTargetPosition); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClient,args.m_bodyUniqueId, args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation,args.m_dt); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); @@ -961,7 +960,7 @@ void b3RobotSimAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const doubl } } -void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition) +void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation) { b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClient,bodyUniqueId); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); @@ -973,5 +972,9 @@ void b3RobotSimAPI::getLinkState(int bodyUniqueId, int linkIndex, double* worldP worldPosition[0] = linkState.m_worldPosition[0]; worldPosition[1] = linkState.m_worldPosition[1]; worldPosition[2] = linkState.m_worldPosition[2]; + worldOrientation[0] = linkState.m_worldOrientation[0]; + worldOrientation[1] = linkState.m_worldOrientation[1]; + worldOrientation[2] = linkState.m_worldOrientation[2]; + worldOrientation[3] = linkState.m_worldOrientation[3]; } } \ No newline at end of file diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index fe640a0a9..bb5fe6d41 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -93,7 +93,8 @@ struct b3RobotSimInverseKinematicArgs // double* m_currentJointPositions; // int m_numPositions; double m_endEffectorTargetPosition[3]; -// double m_endEffectorTargetOrientation[4]; + double m_endEffectorTargetOrientation[4]; + double m_dt; int m_flags; b3RobotSimInverseKinematicArgs() @@ -149,7 +150,7 @@ public: void getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); - void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition); + void getLinkState(int bodyUniqueId, int linkIndex, double* worldPosition, double* worldOrientation); }; #endif //B3_ROBOT_SIM_API_H diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 7c8087a4d..a6c114a44 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -97,9 +97,11 @@ void IKTrajectoryHelper::createKukaIIWA() } bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], + const double endEffectorTargetOrientation[4], const double endEffectorWorldPosition[3], + const double endEffectorWorldOrientation[4], const double* q_current, int numQ, - double* q_new, int ikMethod, const double* linear_jacobian1, int jacobian_size1) + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt) { if (numQ != 7) @@ -127,26 +129,45 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], VectorRn deltaS(3); for (int i = 0; i < 3; ++i) { - deltaS.Set(i,endEffectorTargetPosition[i]-endEffectorWorldPosition[i]); + deltaS.Set(i,(endEffectorTargetPosition[i]-endEffectorWorldPosition[i])/dt); } - m_data->m_ikJacobian->SetDeltaS(deltaS); - + + // Set one end effector world orientation from Bullet + VectorRn deltaR(3); + btQuaternion startQ(endEffectorWorldOrientation[0],endEffectorWorldOrientation[1],endEffectorWorldOrientation[2],endEffectorWorldOrientation[3]); + btQuaternion endQ(endEffectorTargetOrientation[0],endEffectorTargetOrientation[1],endEffectorTargetOrientation[2],endEffectorTargetOrientation[3]); + btQuaternion deltaQ = endQ*startQ.inverse(); + float angle = deltaQ.getAngle(); + btVector3 axis = deltaQ.getAxis(); + float angleDot = angle/dt; + btVector3 angularVel = angleDot*axis.normalize(); + for (int i = 0; i < 3; ++i) + { + deltaR.Set(i,angularVel[i]); + } + + VectorRn deltaC(6); + for (int i = 0; i < 3; ++i) + { + deltaC.Set(i,deltaS[i]); + deltaC.Set(i+3,deltaR[i]); + } + m_data->m_ikJacobian->SetDeltaS(deltaC); + // Set Jacobian from Bullet body Jacobian int nRow = m_data->m_ikJacobian->GetNumRows(); int nCol = m_data->m_ikJacobian->GetNumCols(); - if (jacobian_size1) + b3Assert(jacobian_size==nRow*nCol); + MatrixRmn completeJacobian(nRow,nCol); + for (int i = 0; i < nRow/2; ++i) { - b3Assert(jacobian_size1==nRow*nCol); - MatrixRmn linearJacobian(nRow,nCol); - for (int i = 0; i < nRow; ++i) + for (int j = 0; j < nCol; ++j) { - for (int j = 0; j < nCol; ++j) - { - linearJacobian.Set(i,j,linear_jacobian1[i*nCol+j]); - } + completeJacobian.Set(i,j,linear_jacobian[i*nCol+j]); + completeJacobian.Set(i+nRow/2,j,angular_jacobian[i*nCol+j]); } - m_data->m_ikJacobian->SetJendTrans(linearJacobian); } + m_data->m_ikJacobian->SetJendTrans(completeJacobian); // Calculate the change in theta values switch (ikMethod) { @@ -165,19 +186,22 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], case IK2_SDLS: m_data->m_ikJacobian->CalcDeltaThetasSDLS(); // Selectively damped least squares method break; + case IK2_VEL_DLS: + m_data->m_ikJacobian->CalcThetasDotDLS(dt); // Damped least square for velocity IK + break; default: m_data->m_ikJacobian->ZeroDeltaThetas(); break; } m_data->m_ikJacobian->UpdateThetas(); - + // Apply the change in the theta values - m_data->m_ikJacobian->UpdatedSClampValue(&targets); + //m_data->m_ikJacobian->UpdatedSClampValue(&targets); for (int i=0;im_ikNodes[i]->GetTheta(); + q_new[i] = m_data->m_ikNodes[i]->GetTheta()*dt + q_current[i]; } return true; } diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index bfbc4cdec..ca786a104 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -7,7 +7,8 @@ enum IK2_Method IK2_PURE_PSEUDO, IK2_DLS, IK2_SDLS , - IK2_DLS_SVD + IK2_DLS_SVD, + IK2_VEL_DLS }; @@ -25,9 +26,11 @@ public: bool createFromMultiBody(class btMultiBody* mb); bool computeIK(const double endEffectorTargetPosition[3], + const double endEffectorTargetOrientation[4], const double endEffectorWorldPosition[3], + const double endEffectorWorldOrientation[4], const double* q_old, int numQ, - double* q_new, int ikMethod, const double* linear_jacobian, int jacobian_size); + double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, float dt); }; #endif //IK_TRAJECTORY_HELPER_H diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index b9279938a..d6e113fe2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1319,8 +1319,7 @@ int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJ } ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, - const double targetPosition[3]) +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -1341,6 +1340,12 @@ b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsCli command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0]; command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1]; command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2]; + + command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = targetOrientation[2]; + command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = targetOrientation[3]; + command->m_calculateInverseKinematicsArguments.m_dt = dt; return (b3SharedMemoryCommandHandle)command; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index ba843d5f9..f3d9acd9c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -121,8 +121,7 @@ b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); ///compute the joint positions to move the end effector to a desired target using inverse kinematics -b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, - const double targetPosition[3]); +b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double targetPosition[3], const double targetOrientation[4], const double dt); int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 0639eea64..9779cad8e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2576,6 +2576,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3AlignedObjectArray jacobian_linear; jacobian_linear.resize(3*7); + b3AlignedObjectArray jacobian_angular; + jacobian_angular.resize(3*7); int jacSize = 0; btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); @@ -2605,12 +2607,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { tree->calculateJacobians(q); btInverseDynamics::mat3x jac_t(3, num_dofs); + btInverseDynamics::mat3x jac_r(3,num_dofs); tree->getBodyJacobianTrans(endEffectorLinkIndex, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex, &jac_r); for (int i = 0; i < 3; ++i) { for (int j = 0; j < num_dofs; ++j) { jacobian_linear[i*num_dofs+j] = jac_t(i,j); + jacobian_angular[i*num_dofs+j] = jac_r(i,j); } } } @@ -2618,19 +2623,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm double q_new[7]; - int ikMethod=IK2_DLS; + int ikMethod=IK2_VEL_DLS; btVector3DoubleData endEffectorWorldPosition; - + btVector3DoubleData endEffectorWorldOrientation; btVector3 endEffectorPosWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getOrigin(); + btQuaternion endEffectorOriWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform.getRotation(); + btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); + endEffectorOri.serializeDouble(endEffectorWorldOrientation); - ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, - endEffectorWorldPosition.m_floats, + ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, q_current, - numJoints, q_new, ikMethod, &jacobian_linear[0],jacSize); + numJoints, q_new, ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2,clientCmd.m_calculateInverseKinematicsArguments.m_dt); serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; for (int i=0;iGetNumEffector(); nJoint = tree->GetNumJoint(); - nRow = 3 * nEffector; + nRow = 2 * 3 * nEffector; // Include both linear and angular part nCol = nJoint; Jend.SetSize(nRow, nCol); // The Jocobian matrix @@ -276,6 +276,31 @@ void Jacobian::CalcDeltaThetasDLS() } } +void Jacobian::CalcThetasDotDLS(float dt) +{ + const MatrixRmn& J = ActiveJacobian(); + + MatrixRmn::MultiplyTranspose(J, J, U); // U = J * (J^T) + U.AddToDiagonal( DampingLambdaSq ); + + // Use the next four lines instead of the succeeding two lines for the DLS method with clamped error vector e. + // CalcdTClampedFromdS(); + // VectorRn dTextra(3*nEffector); + // U.Solve( dT, &dTextra ); + // J.MultiplyTranspose( dTextra, dTheta ); + + // Use these two lines for the traditional DLS method + U.Solve( dS, &dT1 ); + J.MultiplyTranspose( dT1, dTheta ); + + // Scale back to not exceed maximum angle changes + double MaxVelDLS = MaxAngleDLS/dt; + double maxChange = dTheta.MaxAbs(); + if ( maxChange>MaxVelDLS ) { + dTheta *= MaxVelDLS/maxChange; + } +} + void Jacobian::CalcDeltaThetasDLSwithSVD() { const MatrixRmn& J = ActiveJacobian(); diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 3f62b2da2..2663e62c5 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -69,6 +69,7 @@ public: void CalcDeltaThetasDLS(); void CalcDeltaThetasDLSwithSVD(); void CalcDeltaThetasSDLS(); + void CalcThetasDotDLS(float dt); void UpdateThetas(); double UpdateErrorArray(VectorR3* targets); // Returns sum of errors From cbda64c5e761a95cea0c1ff83efe93e6884395f5 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 20 Sep 2016 10:24:48 -0700 Subject: [PATCH 2/3] Update theta dot for velocity IK. --- examples/SharedMemory/IKTrajectoryHelper.cpp | 10 +++++++++- examples/ThirdPartyLibs/BussIK/Jacobian.cpp | 18 ++++++++++++++++++ examples/ThirdPartyLibs/BussIK/Jacobian.h | 1 + examples/ThirdPartyLibs/BussIK/Node.h | 4 ++++ 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index a6c114a44..3e4537ea9 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -194,14 +194,22 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], break; } - m_data->m_ikJacobian->UpdateThetas(); + // Use for velocity IK, update theta dot + m_data->m_ikJacobian->UpdateThetaDot(); + + // Use for position IK, incrementally update theta + //m_data->m_ikJacobian->UpdateThetas(); // Apply the change in the theta values //m_data->m_ikJacobian->UpdatedSClampValue(&targets); for (int i=0;im_ikNodes[i]->GetTheta()*dt + q_current[i]; + + // Use for position IK + //q_new[i] = m_data->m_ikNodes[i]->GetTheta(); } return true; } diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index c47fc3cc6..6fd6c6aa6 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -170,6 +170,24 @@ void Jacobian::UpdateThetas() tree->Compute(); } +void Jacobian::UpdateThetaDot() +{ + // Traverse the tree to find all joints + // Update the joint angles + Node* n = tree->GetRoot(); + while ( n ) { + if ( n->IsJoint() ) { + int i = n->GetJointNum(); + n->UpdateTheta( dTheta[i] ); + + } + n = tree->GetSuccessor( n ); + } + + // Update the positions and rotation axes of all joints/effectors + tree->Compute(); +} + void Jacobian::CalcDeltaThetas() { switch (CurrentUpdateMode) { diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 2663e62c5..38aa475c0 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -72,6 +72,7 @@ public: void CalcThetasDotDLS(float dt); void UpdateThetas(); + void UpdateThetaDot(); double UpdateErrorArray(VectorR3* targets); // Returns sum of errors const VectorRn& GetErrorArray() const { return errorArray; } void UpdatedSClampValue(VectorR3* targets); diff --git a/examples/ThirdPartyLibs/BussIK/Node.h b/examples/ThirdPartyLibs/BussIK/Node.h index 5a49d278a..67ee4f91e 100644 --- a/examples/ThirdPartyLibs/BussIK/Node.h +++ b/examples/ThirdPartyLibs/BussIK/Node.h @@ -56,6 +56,10 @@ public: delta = actualDelta; #endif return theta; } + + double UpdateTheta( double& delta ) { + theta = delta; + return theta; } const VectorR3& GetS() const { return s; } const VectorR3& GetW() const { return w; } From c01f6245d08fa7eef1b610e9b8223a0c96fc1913 Mon Sep 17 00:00:00 2001 From: yunfeibai Date: Tue, 20 Sep 2016 11:10:13 -0700 Subject: [PATCH 3/3] Quick fix of pybullet for IK with orientation constraint. --- examples/pybullet/pybullet.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 95a200271..f73c16a36 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1663,6 +1663,8 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self, if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetPosObj)) { double pos[3]; + double ori[4]={0,1.0,0,0}; + double dt=0.0001; if (pybullet_internalSetVectord(targetPosObj,pos)) { @@ -1671,7 +1673,7 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self, int resultBodyIndex; int result; b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex, - pos); + pos,ori,dt); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); result = b3GetStatusInverseKinematicsJointPositions(statusHandle,