From a3e4582bef526d5d808a3fb74b3f379c98fb7b4b Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:23:17 +0200 Subject: [PATCH 1/9] getCollisionShapeData and getVisualShapeData were added to RobotSimulatorClinetAPI_NoGUI. b3RobotSimulatorJointMotorArrayArgs initialization bug fix. --- .../b3RobotSimulatorClientAPI_NoGUI.cpp | 55 ++++++++++++++++++- .../b3RobotSimulatorClientAPI_NoGUI.h | 17 +++++- 2 files changed, 69 insertions(+), 3 deletions(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index cc32a3913..760670382 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1626,6 +1626,10 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); } + if (args.m_restitutionVelocityThreshold >= 0) { + b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); return true; } @@ -2060,4 +2064,53 @@ void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* gu struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() { return m_data->m_guiHelper; -} \ No newline at end of file +} + +bool b3RobotSimulatorClientAPI_NoGUI::getCollisionShapeData(int bodyUniqueId, int linkIndex, + b3CollisionShapeInformation &collisionShapeInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + command = b3InitRequestCollisionShapeInformation(sm, bodyUniqueId, linkIndex); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + statusType = b3GetStatusType(statusHandle); + } + + btAssert(statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED); + if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED) { + b3GetCollisionShapeInformation(sm, &collisionShapeInfo); + } + return true; +} + +bool b3RobotSimulatorClientAPI_NoGUI::getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo) +{ + b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; + if (sm == 0) { + b3Warning("Not connected"); + return false; + } + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + + { + commandHandle = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + btAssert(statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED); + if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { + b3GetVisualShapeInformation(sm, &visualShapeInfo); + } + return true; + } +} diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h index 02594cf40..2a3dcc328 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.h @@ -152,7 +152,14 @@ struct b3RobotSimulatorJointMotorArrayArgs double *m_forces; b3RobotSimulatorJointMotorArrayArgs(int controlMode, int numControlledDofs) - : m_controlMode(controlMode), m_numControlledDofs(numControlledDofs) + : m_controlMode(controlMode), + m_numControlledDofs(numControlledDofs), + m_jointIndices(NULL), + m_targetPositions(NULL), + m_kps(NULL), + m_targetVelocities(NULL), + m_kds(NULL), + m_forces(NULL) { } }; @@ -204,6 +211,7 @@ struct b3RobotSimulatorSetPhysicsEngineParameters double m_erp; double m_contactERP; double m_frictionERP; + double m_solverResidualThreshold; b3RobotSimulatorSetPhysicsEngineParameters() : m_fixedTimeStep(-1), @@ -218,7 +226,8 @@ struct b3RobotSimulatorSetPhysicsEngineParameters m_restitutionVelocityThreshold(-1), m_erp(-1), m_contactERP(-1), - m_frictionERP(-1) + m_frictionERP(-1), + m_solverResidualThreshold(-1) {} }; @@ -566,6 +575,10 @@ public: virtual void setGuiHelper(struct GUIHelperInterface* guiHelper); virtual struct GUIHelperInterface* getGuiHelper(); + + bool getCollisionShapeData(int bodyUniqueId, int linkIndex, b3CollisionShapeInformation &collisionShapeInfo); + + bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo); }; From 6bd7a52bf8d2c43ec4cfa95972b6dbd91c5c8ffe Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:25:28 +0200 Subject: [PATCH 2/9] solverResidualThreshold now can be set. --- examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index 760670382..dfe6e19a2 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1626,7 +1626,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSi b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP); } - if (args.m_restitutionVelocityThreshold >= 0) { + if (args.m_solverResidualThreshold >= 0) { b3PhysicsParamSetSolverResidualThreshold(command, args.m_solverResidualThreshold); } From 325ccad2582257d30105857fef6ed0067f718118 Mon Sep 17 00:00:00 2001 From: donghokang Date: Tue, 29 May 2018 16:41:37 +0200 Subject: [PATCH 3/9] getDynamicsInfo function now works. Changed to call b3GetDynamicsInfo in getDynamicsInfo. --- examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index dfe6e19a2..ee6a20989 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1270,6 +1270,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int link status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle); status_type = b3GetStatusType(status_handle); if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) { + b3GetDynamicsInfo(status_handle, dynamicsInfo); return true; } else { b3Warning("getDynamicsInfo did not complete"); From edc70582dd6fa731f697ffdd7e629fd80c29b52e Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 31 May 2018 16:06:15 -0700 Subject: [PATCH 4/9] implement accurate inverse kinematics in C++. PyBullet.calculateInverseKinematics gets "maxNumIterations=20", "residualThreshold=1.04" to tune allow to provide current joint positions in IK, overriding the body joint positions, also IK target will be in local coordinates. expose b3ComputeDofCount in C-API --- examples/SharedMemory/PhysicsClientC_API.cpp | 108 ++++- examples/SharedMemory/PhysicsClientC_API.h | 27 +- .../PhysicsServerCommandProcessor.cpp | 435 +++++++++++------- examples/SharedMemory/SharedMemoryCommands.h | 3 + examples/SharedMemory/SharedMemoryPublic.h | 4 +- .../pybullet/examples/inverse_kinematics.py | 6 +- examples/pybullet/pybullet.c | 93 ++-- 7 files changed, 429 insertions(+), 247 deletions(-) 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 Date: Thu, 31 May 2018 17:58:12 -0700 Subject: [PATCH 5/9] fix different arg name in header versus cpp --- .../b3RobotSimulatorClientAPI_NoGUI.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp index cc32a3913..7e7dabafe 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI_NoGUI.cpp @@ -1396,7 +1396,7 @@ bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) { } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) { b3PhysicsClientHandle sm = m_data->m_physicsClientHandle; if (sm == 0) { @@ -1407,7 +1407,7 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ b3SharedMemoryStatusHandle statusHandle; int statusType; - commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); + commandHandle = b3InitUserDebugDrawAddText3D(sm, text, textPosition, &args.m_colorRGB[0], args.m_size, args.m_lifeTime); if (args.m_parentObjectUniqueId>=0) { b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex); @@ -1428,12 +1428,12 @@ int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ return -1; } -int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args) +int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &textPosition, struct b3RobotSimulatorAddUserDebugTextArgs & args) { double dposXYZ[3]; - dposXYZ[0] = posXYZ.x(); - dposXYZ[1] = posXYZ.y(); - dposXYZ[2] = posXYZ.z(); + dposXYZ[0] = textPosition.x(); + dposXYZ[1] = textPosition.y(); + dposXYZ[2] = textPosition.z(); return addUserDebugText(text, &dposXYZ[0], args); } @@ -2060,4 +2060,4 @@ void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* gu struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper() { return m_data->m_guiHelper; -} \ No newline at end of file +} From 76e88dddc55dbdb6f5aab7014c07550703790311 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 31 May 2018 18:16:40 -0700 Subject: [PATCH 6/9] add space to make conversion work --- examples/pybullet/gym/pybullet_envs/robot_bases.py | 2 +- examples/pybullet/gym/pybullet_envs/robot_locomotors.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/robot_bases.py b/examples/pybullet/gym/pybullet_envs/robot_bases.py index 25d079831..f9b39b347 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_bases.py +++ b/examples/pybullet/gym/pybullet_envs/robot_bases.py @@ -1,4 +1,4 @@ -import pybullet +import pybullet import gym, gym.spaces, gym.utils import numpy as np import os, inspect diff --git a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py index 4db807f5e..7ed7a1272 100644 --- a/examples/pybullet/gym/pybullet_envs/robot_locomotors.py +++ b/examples/pybullet/gym/pybullet_envs/robot_locomotors.py @@ -1,6 +1,6 @@ from robot_bases import XmlBasedRobot, MJCFBasedRobot, URDFBasedRobot import numpy as np -import pybullet +import pybullet import os import pybullet_data from robot_bases import BodyPart From 4c75e022c8ec53dcc91ae416f59e2691db34247b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 31 May 2018 21:07:04 -0700 Subject: [PATCH 7/9] Use dofCount and not numJoints in PyBullet.calculateInverseKinematics, fixes null space demo See baxter_ik_demo at https://github.com/erwincoumans/pybullet_robots --- examples/pybullet/pybullet.c | 43 ++++++++++++++---------------------- 1 file changed, 16 insertions(+), 27 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0e27316fb..d6954e0b8 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -8123,36 +8123,32 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, double* jointDamping = 0; double* currentPositions = 0; - if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && - (szJointRanges == numJoints) && (szRestPoses == numJoints)) + if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) && + (szJointRanges == dofCount) && (szRestPoses == dofCount)) { - int szInBytes = sizeof(double) * numJoints; + int szInBytes = sizeof(double) * dofCount; int i; lowerLimits = (double*)malloc(szInBytes); upperLimits = (double*)malloc(szInBytes); jointRanges = (double*)malloc(szInBytes); restPoses = (double*)malloc(szInBytes); - for (i = 0; i < numJoints; i++) + for (i = 0; i < dofCount; i++) { - lowerLimits[i] = - pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); - upperLimits[i] = - pybullet_internalGetFloatFromSequence(upperLimitsObj, i); - jointRanges[i] = - pybullet_internalGetFloatFromSequence(jointRangesObj, i); - restPoses[i] = - pybullet_internalGetFloatFromSequence(restPosesObj, i); + lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i); } hasNullSpace = 1; } if (szCurrentPositions > 0) { - if (szCurrentPositions < numJoints) + if (szCurrentPositions != dofCount) { PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input current positions is smaller than the number of joints."); + "calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom."); return NULL; } else @@ -8170,17 +8166,10 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (szJointDamping > 0) { - // We allow the number of joint damping values to be larger than - // the number of degrees of freedom (DOFs). On the server side, it does - // the check and only sets joint damping for all DOFs. - // We can use the number of DOFs here when that is exposed. Since the - // number of joints is larger than the number of DOFs (we assume the - // joint is either fixed joint or one DOF joint), it is safe to use - // number of joints here. - if (szJointDamping < numJoints) + if (szJointDamping != dofCount) { PyErr_SetString(SpamError, - "calculateInverseKinematics the size of input joint damping values is smaller than the number of joints."); + "calculateInverseKinematics the size of input joint damping values is unequal to the number of degrees of freedom."); return NULL; } else @@ -8208,7 +8197,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (hasCurrentPositions) { - b3CalculateInverseKinematicsSetCurrentPositions(command, numJoints, currentPositions); + b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions); } if (maxNumIterations>0) { @@ -8223,11 +8212,11 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { if (hasOrn) { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); } else { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); } } else @@ -8244,7 +8233,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, if (hasJointDamping) { - b3CalculateInverseKinematicsSetJointDamping(command, numJoints, jointDamping); + b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping); } free(currentPositions); free(jointDamping); From 7653ddbeba1958f0e4bc939ba75e887918a16c1d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Thu, 31 May 2018 21:17:39 -0700 Subject: [PATCH 8/9] bump up pybullet version (for fixed ik) --- setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 7a3b2f443..90c0875fe 100644 --- a/setup.py +++ b/setup.py @@ -450,7 +450,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.1', + version='2.0.2', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', From e74cd05e37494fd42a94b3e3d6a47ad01cabd10b Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 31 May 2018 21:21:50 -0700 Subject: [PATCH 9/9] add space --- examples/pybullet/gym/pybullet_envs/examples/ANYmal.py | 4 ++-- examples/pybullet/gym/pybullet_utils/bullet_client.py | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py index e413d3971..90f80a754 100644 --- a/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py +++ b/examples/pybullet/gym/pybullet_envs/examples/ANYmal.py @@ -24,8 +24,8 @@ ground = p.loadURDF("plane.urdf",[0,0,0], flags=p.URDF_ENABLE_CACHED_GRAPHICS_SH p.setPhysicsEngineParameter(solverResidualThreshold=1e-2) index = 0 -numX = 3 -numY = 3 +numX = 10 +numY = 10 for i in range (numX): for j in range (numY): diff --git a/examples/pybullet/gym/pybullet_utils/bullet_client.py b/examples/pybullet/gym/pybullet_utils/bullet_client.py index 8fc1fcd5c..9b7f907f3 100644 --- a/examples/pybullet/gym/pybullet_utils/bullet_client.py +++ b/examples/pybullet/gym/pybullet_utils/bullet_client.py @@ -3,7 +3,7 @@ from __future__ import absolute_import from __future__ import division import functools import inspect -import pybullet +import pybullet class BulletClient(object):