diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index eae4255a6..afa1da448 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1516,7 +1516,7 @@ B3_SHARED_API int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHan return 0; } -B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) +B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1526,7 +1526,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClien b3Assert(command); command->m_type = CMD_INIT_POSE; command->m_updateFlags =0; - command->m_initPoseArgs.m_bodyUniqueId = bodyIndex; + command->m_initPoseArgs.m_bodyUniqueId = bodyUniqueId; //a bit slow, initialing the full range to zero... for (int i=0;igetNumJoints(bodyId); + return cl->getNumJoints(bodyUniqueId); } -B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info) +B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId) +{ + int nj = b3GetNumJoints(physClient, bodyUniqueId); + int j=0; + int dofCountOrg = 0; + for (j=0;jgetJointInfo(bodyIndex, jointIndex, *info); + return cl->getJointInfo(bodyUniqueId, jointIndex, *info); } @@ -2389,7 +2427,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo -B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info) +B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info) { PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); @@ -2400,9 +2438,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3Ph command->m_type = CMD_USER_CONSTRAINT; command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT; - command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex; + command->m_userConstraintArguments.m_parentBodyIndex = parentBodyUniqueId; command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex; - command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex; + command->m_userConstraintArguments.m_childBodyIndex = childBodyUniqueId; command->m_userConstraintArguments.m_childJointIndex = childJointIndex; for (int i = 0; i < 7; ++i) { command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i]; @@ -3742,7 +3780,7 @@ B3_SHARED_API void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHand ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) { PhysicsClient* cl = (PhysicsClient*)physClient; @@ -3753,8 +3791,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit( command->m_type = CMD_CALCULATE_INVERSE_DYNAMICS; command->m_updateFlags = 0; - command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyIndex; - int numJoints = cl->getNumJoints(bodyIndex); + command->m_calculateInverseDynamicsArguments.m_bodyUniqueId = bodyUniqueId; + int numJoints = cl->getNumJoints(bodyUniqueId); for (int i = 0; i < numJoints;i++) { command->m_calculateInverseDynamicsArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; @@ -3799,7 +3837,7 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand return true; } -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3809,12 +3847,12 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi command->m_type = CMD_CALCULATE_JACOBIAN; command->m_updateFlags = 0; - command->m_calculateJacobianArguments.m_bodyUniqueId = bodyIndex; + command->m_calculateJacobianArguments.m_bodyUniqueId = bodyUniqueId; command->m_calculateJacobianArguments.m_linkIndex = linkIndex; command->m_calculateJacobianArguments.m_localPosition[0] = localPosition[0]; command->m_calculateJacobianArguments.m_localPosition[1] = localPosition[1]; command->m_calculateJacobianArguments.m_localPosition[2] = localPosition[2]; - int numJoints = cl->getNumJoints(bodyIndex); + int numJoints = cl->getNumJoints(bodyUniqueId); for (int i = 0; i < numJoints;i++) { command->m_calculateJacobianArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; @@ -3859,7 +3897,7 @@ B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, i return true; } -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3869,7 +3907,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3Phy command->m_type = CMD_CALCULATE_MASS_MATRIX; command->m_updateFlags = 0; - int numJoints = cl->getNumJoints(bodyIndex); + int numJoints = cl->getNumJoints(bodyUniqueId); for (int i = 0; i < numJoints; i++) { command->m_calculateMassMatrixArguments.m_jointPositionsQ[i] = jointPositionsQ[i]; @@ -3905,7 +3943,7 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar } ///compute the joint positions to move the end effector to a desired target using inverse kinematics -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); @@ -3915,7 +3953,7 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandIni command->m_type = CMD_CALCULATE_INVERSE_KINEMATICS; command->m_updateFlags = 0; - command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyIndex; + command->m_calculateInverseKinematicsArguments.m_bodyUniqueId = bodyUniqueId; return (b3SharedMemoryCommandHandle)command; @@ -4008,6 +4046,36 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe } +B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_CURRENT_JOINT_POSITIONS; + for (int i = 0; i < numDof; ++i) + { + command->m_calculateInverseKinematicsArguments.m_currentPositions[i] = currentJointPositions[i]; + } +} + +B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_MAX_ITERATIONS; + command->m_calculateInverseKinematicsArguments.m_maxNumIterations = maxNumIterations; +} + +B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_RESIDUAL_THRESHOLD; + command->m_calculateInverseKinematicsArguments.m_residualThreshold = residualThreshold; +} + B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -4513,7 +4581,7 @@ B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient) return -1; } -B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path) +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path) { PhysicsClient* cl = (PhysicsClient*)physClient; b3Assert(cl); diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 712340dc9..3c23c2003 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -104,10 +104,14 @@ B3_SHARED_API int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serial B3_SHARED_API int b3GetBodyInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, struct b3BodyInfo* info); ///give a unique body index (after loading the body) return the number of joints. -B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyId); +B3_SHARED_API int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyUniqueId); + +///compute the number of degrees of freedom for this body. +///Return -1 for unsupported spherical joint, -2 for unsupported planar joint. +B3_SHARED_API int b3ComputeDofCount(b3PhysicsClientHandle physClient, int bodyUniqueId); ///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h -B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info); +B3_SHARED_API int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int jointIndex, struct b3JointInfo* info); B3_SHARED_API b3SharedMemoryCommandHandle b3GetDynamicsInfoCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex); ///given a body unique id and link index, return the dynamics information. See b3DynamicsInfo in SharedMemoryPublic.h @@ -128,7 +132,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHan B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius); B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold); -B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); +B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info); ///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); @@ -350,26 +354,26 @@ B3_SHARED_API void b3LoadMJCFCommandSetFlags(b3SharedMemoryCommandHandle command ///compute the forces to achieve an acceleration, given a state q and qdot using inverse dynamics -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseDynamicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHandle statusHandle, int* bodyUniqueId, int* dofCount, double* jointForces); -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian); -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, const double* jointPositionsQ); +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateMassMatrixCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, const double* jointPositionsQ); ///the mass matrix is stored in column-major layout of size dofCount*dofCount B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* massMatrix); ///compute the joint positions to move the end effector to a desired target using inverse kinematics -B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); +B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]); B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose); @@ -381,6 +385,11 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu int* dofCount, double* jointPositions); +B3_SHARED_API void b3CalculateInverseKinematicsSetCurrentPositions(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* currentJointPositions); +B3_SHARED_API void b3CalculateInverseKinematicsSetMaxNumIterations(b3SharedMemoryCommandHandle commandHandle, int maxNumIterations); +B3_SHARED_API void b3CalculateInverseKinematicsSetResidualThreshold(b3SharedMemoryCommandHandle commandHandle, double residualThreshold); + + B3_SHARED_API b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName); B3_SHARED_API int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody); B3_SHARED_API int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling); @@ -476,7 +485,7 @@ B3_SHARED_API int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle com ///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position, ///base orientation and joint angles. This will set all velocities of base and joints to zero. ///This is not a robot control command using actuators/joint motors, but manual repositioning the robot. -B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); +B3_SHARED_API b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); B3_SHARED_API int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); B3_SHARED_API int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW); B3_SHARED_API int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[/*3*/]); @@ -575,7 +584,7 @@ B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryComma B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds); B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient); -B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, char* path); +B3_SHARED_API b3SharedMemoryCommandHandle b3SetAdditionalSearchPath(b3PhysicsClientHandle physClient, const char* path); B3_SHARED_API void b3MultiplyTransforms(const double posA[/*3*/], const double ornA[/*4*/], const double posB[/*3*/], const double ornB[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); B3_SHARED_API void b3InvertTransform(const double pos[/*3*/], const double orn[/*4*/], double outPos[/*3*/], double outOrn[/*4*/]); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index fa8f8da40..ab9dbcfc8 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6904,7 +6904,8 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe } } - + + btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; @@ -8201,201 +8202,295 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + btAlignedObjectArray startingPositions; + startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks()); + + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); + + + btTransform targetBaseCoord; + if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS) { - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + targetBaseCoord.setOrigin(targetPosWorld); + targetBaseCoord.setRotation(targetOrnWorld); + } else + { + btTransform targetWorld; + targetWorld.setOrigin(targetPosWorld); + targetWorld.setRotation(targetOrnWorld); + btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + targetBaseCoord = tr.inverse()*targetWorld; + } + + + { + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) + { + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) + { + // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + double curPos = 0; + if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS) + { + curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex]; + } else + { + curPos = bodyHandle->m_multiBody->getJointPos(i); + } + startingPositions.push_back(curPos); + DofIndex++; + } + } + } + + int numIterations = 20; + if (clientCmd.m_updateFlags& IK_HAS_MAX_ITERATIONS) + { + numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations; + } + double residualThreshold = 1e-4; + if (clientCmd.m_updateFlags& IK_HAS_RESIDUAL_THRESHOLD) + { + residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold; + } + + btScalar currentDiff = 1e30f; + b3AlignedObjectArray jacobian_linear; + b3AlignedObjectArray jacobian_angular; + btAlignedObjectArray q_current; + btAlignedObjectArray q_new; + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + + for (int i=0;i residualThreshold;i++) + { + BT_PROFILE("InverseKinematics1Step"); + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + + + jacobian_linear.resize(3*numDofs); + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - btAlignedObjectArray q_current; - q_current.resize(numDofs); - - - - if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) - { - jacSize = jacobian_linear.size(); - // Set jacobian value + q_current.resize(numDofs); + + + if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) + { + btInverseDynamics::vec3 world_origin; + btInverseDynamics::mat33 world_rot; + + jacSize = jacobian_linear.size(); + // Set jacobian value - - - btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); - int DofIndex = 0; - for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. - q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); - q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); - qdot[DofIndex+baseDofs] = 0; - nu[DofIndex+baseDofs] = 0; - DofIndex++; - } - } // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); - btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. - tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); - tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); - jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); - } - } - } - - - - btAlignedObjectArray q_new; - q_new.resize(numDofs); - int ikMethod = 0; - if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; - } - else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) - { - if (clientCmd.m_updateFlags & IK_SDLS) + + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) + { + // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. + + double curPos = startingPositions[DofIndex]; + q_current[DofIndex] = curPos; + q[DofIndex+baseDofs] = curPos; + qdot[DofIndex+baseDofs] = 0; + nu[DofIndex+baseDofs] = 0; + DofIndex++; + } + } // Set the gravity to correspond to the world gravity + btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); + + { + BT_PROFILE("calculateInverseDynamics"); + if (-1 != tree->setGravityInWorldFrame(id_grav) && + -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) + { + tree->calculateJacobians(q); + btInverseDynamics::mat3x jac_t(3, numDofs+ baseDofs); + btInverseDynamics::mat3x jac_r(3,numDofs + baseDofs); + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); + + //calculatePositionKinematics is already done inside calculateInverseDynamics + tree->getBodyOrigin(endEffectorLinkIndex+1,&world_origin); + tree->getBodyTransform(endEffectorLinkIndex+1,&world_rot); + + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); + } + } + } + } + + + + + q_new.resize(numDofs); + int ikMethod = 0; + if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE; + } + else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION) + { + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + } + else + { + ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + } + } + else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) + { + //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. + ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; } else { - ikMethod = IK2_VEL_DLS_WITH_ORIENTATION; + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS; + } + else + { + ikMethod = IK2_VEL_DLS;; + } } - } - else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - //Nullspace task only works with DLS now. TODO: add nullspace task to SDLS. - ikMethod = IK2_VEL_DLS_WITH_NULLSPACE; - } - else - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS; - } - else - { - ikMethod = IK2_VEL_DLS;; - } - } - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray rest_pose; - lower_limit.resize(numDofs); - upper_limit.resize(numDofs); - joint_range.resize(numDofs); - rest_pose.resize(numDofs); - for (int i = 0; i < numDofs; ++i) + if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) { - lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; - upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + + lower_limit.resize(numDofs); + upper_limit.resize(numDofs); + joint_range.resize(numDofs); + rest_pose.resize(numDofs); + for (int i = 0; i < numDofs; ++i) + { + lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; + upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; + joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; + rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + } + { + BT_PROFILE("computeNullspaceVel"); + ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); + } } - ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); - } - - btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + + + //btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); - btVector3DoubleData endEffectorWorldPosition; - btQuaternionDoubleData endEffectorWorldOrientation; + btVector3DoubleData endEffectorWorldPosition; + btQuaternionDoubleData endEffectorWorldOrientation; + + //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld + btVector3 endEffectorPosWorldOrg = world_origin; + btQuaternion endEffectorOriWorldOrg; + world_rot.getRotation(endEffectorOriWorldOrg); + + btTransform endEffectorBaseCoord; + endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg); + endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg); + + //don't need the next two lines + //btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); + //endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv; + + //btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); + //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld; + //endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld; + + btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); + + //btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); - btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); - btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); - btTransform endEffectorWorld; - endEffectorWorld.setOrigin(endEffectorPosWorldOrg); - endEffectorWorld.setRotation(endEffectorOriWorldOrg); - - btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); - - btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; - - btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); - - btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); - endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - - btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]); + + + + //diff + currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length(); - btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2], - clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); - btTransform targetWorld; - targetWorld.setOrigin(targetPosWorld); - targetWorld.setRotation(targetOrnWorld); - btTransform targetBaseCoord; - targetBaseCoord = tr.inverse()*targetWorld; - btVector3DoubleData targetPosBaseCoord; - btQuaternionDoubleData targetOrnBaseCoord; - targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); - targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); - // Set joint damping coefficents. A small default - // damping constant is added to prevent singularity - // with pseudo inverse. The user can set joint damping - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) + // Set joint damping coefficents. A small default + // damping constant is added to prevent singularity + // with pseudo inverse. The user can set joint damping + // coefficients differently for each joint. The larger + // the damping coefficient is, the less we rely on + // this joint to achieve the IK target. + btAlignedObjectArray joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + for (int i = 0; i < numDofs; ++i) + { + joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + } + } + ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); + + double targetDampCoeff[6]={1.0,1.0,1.0,1.0,1.0,1.0}; + + { + BT_PROFILE("computeIK"); + ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, + endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); + } + serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; + for (int i=0;isetDampingCoeff(numDofs, &joint_damping[0]); - - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, - endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, - &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, - &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); - - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i=0;isetMaxAppliedImpulse(2*scaling); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index bd3285351..d35d425cc 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -695,6 +695,9 @@ struct CalculateInverseKinematicsArgs double m_jointRange[MAX_DEGREE_OF_FREEDOM]; double m_restPose[MAX_DEGREE_OF_FREEDOM]; double m_jointDamping[MAX_DEGREE_OF_FREEDOM]; + double m_currentPositions[MAX_DEGREE_OF_FREEDOM]; + int m_maxNumIterations; + double m_residualThreshold; }; struct CalculateInverseKinematicsResultArgs diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 80bbb3c30..eb64bbc6a 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -628,7 +628,9 @@ enum EnumCalculateInverseKinematicsFlags IK_HAS_TARGET_ORIENTATION=32, IK_HAS_NULL_SPACE_VELOCITY=64, IK_HAS_JOINT_DAMPING=128, - //IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet + IK_HAS_CURRENT_JOINT_POSITIONS=256, + IK_HAS_MAX_ITERATIONS=512, + IK_HAS_RESIDUAL_THRESHOLD = 1024, }; enum b3ConfigureDebugVisualizerEnum diff --git a/examples/pybullet/examples/inverse_kinematics.py b/examples/pybullet/examples/inverse_kinematics.py index 928389542..7293c15c2 100644 --- a/examples/pybullet/examples/inverse_kinematics.py +++ b/examples/pybullet/examples/inverse_kinematics.py @@ -34,12 +34,12 @@ t=0. prevPose=[0,0,0] prevPose1=[0,0,0] hasPrevPose = 0 -useNullSpace = 0 +useNullSpace = 1 useOrientation = 1 #If we set useSimulation=0, it sets the arm pose to be the IK result directly without using dynamic control. #This can be used to test the IK result accuracy. -useSimulation = 1 +useSimulation = 0 useRealTimeSimulation = 1 ikSolver = 0 p.setRealTimeSimulation(useRealTimeSimulation) @@ -69,7 +69,7 @@ while 1: jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) else: if (useOrientation==1): - jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver) + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,jointDamping=jd,solver=ikSolver, maxNumIterations=100, residualThreshold=.01) else: jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,solver=ikSolver) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 31b201ea0..c1c812a0c 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8055,9 +8055,6 @@ static PyObject* pybullet_executePluginCommand(PyObject* self, return PyInt_FromLong(statusType); } - - - ///Inverse Kinematics binding static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* args, PyObject* keywds) @@ -8077,9 +8074,12 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* jointRangesObj = 0; PyObject* restPosesObj = 0; PyObject* jointDampingObj = 0; + PyObject* currentPositionsObj = 0; + int maxNumIterations = -1; + double residualThreshold=-1; - static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOii", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndex, &targetPosObj, &targetOrnObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, ¤tPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId)) { //backward compatibility bodyIndex -> bodyUniqueId. don't update keywords, people need to migrate to bodyUniqueId version static char* kwlist2[] = {"bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "physicsClientId", NULL}; @@ -8107,16 +8107,22 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; + int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0; + + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + int dofCount = b3ComputeDofCount(sm, bodyUniqueId); int hasNullSpace = 0; int hasJointDamping = 0; + int hasCurrentPositions = 0; double* lowerLimits = 0; double* upperLimits = 0; double* jointRanges = 0; double* restPoses = 0; double* jointDamping = 0; - + double* currentPositions = 0; + if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && (szJointRanges == numJoints) && (szRestPoses == numJoints)) { @@ -8141,6 +8147,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, hasNullSpace = 1; } + if (szCurrentPositions > 0) + { + if (szCurrentPositions < numJoints) + { + PyErr_SetString(SpamError, + "calculateInverseKinematics the size of input current positions is smaller than the number of joints."); + return NULL; + } + else + { + int szInBytes = sizeof(double) * szCurrentPositions; + int i; + currentPositions = (double*)malloc(szInBytes); + for (i = 0; i < szCurrentPositions; i++) + { + currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i); + } + hasCurrentPositions = 1; + } + } + if (szJointDamping > 0) { // We allow the number of joint damping values to be larger than @@ -8179,6 +8206,19 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); b3CalculateInverseKinematicsSelectSolver(command, solver); + if (hasCurrentPositions) + { + b3CalculateInverseKinematicsSetCurrentPositions(command, numJoints, currentPositions); + } + if (maxNumIterations>0) + { + b3CalculateInverseKinematicsSetMaxNumIterations(command,maxNumIterations); + } + if (residualThreshold>=0) + { + b3CalculateInverseKinematicsSetResidualThreshold(command,residualThreshold); + } + if (hasNullSpace) { if (hasOrn) @@ -8206,6 +8246,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); } + free(currentPositions); free(jointDamping); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); @@ -8294,44 +8335,8 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* arg int szObPos = PySequence_Size(objPositionsQ); int szObVel = PySequence_Size(objVelocitiesQdot); int szObAcc = PySequence_Size(objAccelerations); - int nj = b3GetNumJoints(sm, bodyUniqueId); - int j=0; - int dofCountOrg = 0; - for (j=0;j