From 39a4e8dcd962099d10c632e747f5b3ba6074e039 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sun, 21 Jul 2019 13:08:22 -0700 Subject: [PATCH] Implement faster array versions of PyBullet: getJointStatesMultiDof, getLinkStates, setJointMotorControlMultiDofArray, resetJointStatesMultiDof, Implement StablePD in C++ through setJointMotorControlMultiDofArray method for pybullet_envs.deep_mimic, see testHumanoid.py and examples/pybullet/examples/humanoidMotionCapture.py Minor fix in ChromeTraceUtil in case startTime>endTime (why would it happen?) --- examples/SharedMemory/PhysicsClientC_API.cpp | 10 + examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 311 ++++-- examples/SharedMemory/SharedMemoryPublic.h | 1 + .../plugins/stablePDPlugin/RBDModel.cpp | 27 +- examples/Utils/ChromeTraceUtil.cpp | 10 +- .../examples/humanoidMotionCapture.py | 80 +- .../deep_mimic/env/humanoid_stable_pd.py | 259 +++-- .../deep_mimic/env/pybullet_deep_mimic_env.py | 12 +- .../deep_mimic/env/testHumanoid.py | 18 +- .../pybullet_utils/pd_controller_stable.py | 11 +- examples/pybullet/pybullet.c | 934 +++++++++++++++++- 12 files changed, 1503 insertions(+), 171 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index d2c958e2a..1b29da628 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -856,7 +856,17 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit2Internal(b3S for (int i = 0; i < MAX_DEGREE_OF_FREEDOM; i++) { command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0; + } + for (int i = 0; i < 7; i++) + { + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[i] = 0; + command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0; + command->m_sendDesiredStateCommandArgument.m_Kp[i] = 0; + command->m_sendDesiredStateCommandArgument.m_Kd[i] = 0; + command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0; + } + command->m_sendDesiredStateCommandArgument.m_desiredStateQ[3] = 1; return (b3SharedMemoryCommandHandle)command; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 9f765cbd3..d3b9ecbb1 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -472,6 +472,7 @@ extern "C" ///Only use when controlMode is CONTROL_MODE_VELOCITY B3_SHARED_API int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount); + B3_SHARED_API int b3JointControlSetDesiredVelocityMultiDof2(b3SharedMemoryCommandHandle commandHandle, int dofIndex, const double* velocity, int dofCount); B3_SHARED_API int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); B3_SHARED_API int b3JointControlSetDesiredForceTorqueMultiDof(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double* forces, int dofCount); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 14376d52c..8da26b469 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1788,32 +1788,11 @@ struct PhysicsServerCommandProcessorInternalData #ifdef STATIC_LINK_SPD_PLUGIN b3HashMap m_rbdModels; - cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot) + static void convertPose(const btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot, Eigen::VectorXd& pose, Eigen::VectorXd&vel) { - cRBDModel* rbdModel = 0; - cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody); - if (rbdModelPtr) - { - rbdModel = *rbdModelPtr; - } - else - { - rbdModel = new cRBDModel(); - Eigen::MatrixXd bodyDefs; - Eigen::MatrixXd jointMat; - btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat); - btVector3 grav = m_dynamicsWorld->getGravity(); - tVector3 gravity(grav[0], grav[1], grav[2], 0); - rbdModel->Init(jointMat, bodyDefs, gravity); - m_rbdModels.insert(multiBody, rbdModel); - } - - //sync pose and vel - int baseDofQ = multiBody->hasFixedBase() ? 0 : 7; int baseDofQdot = multiBody->hasFixedBase() ? 0 : 6; - - Eigen::VectorXd pose, vel; + pose.resize(7 + multiBody->getNumPosVars()); vel.resize(7 + multiBody->getNumPosVars()); //?? @@ -1875,46 +1854,75 @@ struct PhysicsServerCommandProcessorInternalData { switch (multiBody->getLink(l).m_jointType) { - case btMultibodyLink::eRevolute: - case btMultibodyLink::ePrismatic: - { - pose[dof++] = jointPositionsQ[dofsrc++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - break; - } - case btMultibodyLink::eSpherical: - { - double quatXYZW[4]; - quatXYZW[0] = jointPositionsQ[dofsrc++]; - quatXYZW[1] = jointPositionsQ[dofsrc++]; - quatXYZW[2] = jointPositionsQ[dofsrc++]; - quatXYZW[3] = jointPositionsQ[dofsrc++]; + case btMultibodyLink::eRevolute: + case btMultibodyLink::ePrismatic: + { + pose[dof++] = jointPositionsQ[dofsrc++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eSpherical: + { + double quatXYZW[4]; + quatXYZW[0] = jointPositionsQ[dofsrc++]; + quatXYZW[1] = jointPositionsQ[dofsrc++]; + quatXYZW[2] = jointPositionsQ[dofsrc++]; + quatXYZW[3] = jointPositionsQ[dofsrc++]; - pose[dof++] = quatXYZW[3]; - pose[dof++] = quatXYZW[0]; - pose[dof++] = quatXYZW[1]; - pose[dof++] = quatXYZW[2]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; - break; - } - case btMultibodyLink::eFixed: - { - break; - } - default: - { - assert(0); - } + pose[dof++] = quatXYZW[3]; + pose[dof++] = quatXYZW[0]; + pose[dof++] = quatXYZW[1]; + pose[dof++] = quatXYZW[2]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + vel[veldof++] = jointVelocitiesQdot[velsrcdof++]; + break; + } + case btMultibodyLink::eFixed: + { + break; + } + default: + { + assert(0); + } } } + } + + cRBDModel* findOrCreateRBDModel(btMultiBody* multiBody, const double* jointPositionsQ, const double* jointVelocitiesQdot) + { + cRBDModel* rbdModel = 0; + cRBDModel** rbdModelPtr = m_rbdModels.find(multiBody); + if (rbdModelPtr) + { + rbdModel = *rbdModelPtr; + } + else + { + rbdModel = new cRBDModel(); + Eigen::MatrixXd bodyDefs; + Eigen::MatrixXd jointMat; + btExtractJointBodyFromBullet(multiBody, bodyDefs, jointMat); + btVector3 grav = m_dynamicsWorld->getGravity(); + tVector3 gravity(grav[0], grav[1], grav[2], 0); + rbdModel->Init(jointMat, bodyDefs, gravity); + m_rbdModels.insert(multiBody, rbdModel); + } + + //sync pose and vel + + Eigen::VectorXd pose, vel; + PhysicsServerCommandProcessorInternalData::convertPose(multiBody, jointPositionsQ, jointVelocitiesQdot, pose, vel); btVector3 gravOrg = m_dynamicsWorld->getGravity(); tVector grav(gravOrg[0], gravOrg[1], gravOrg[2], 0); rbdModel->SetGravity(grav); - rbdModel->Update(pose, vel); + { + BT_PROFILE("rbdModel::Update"); + rbdModel->Update(pose, vel); + } return rbdModel; } @@ -6168,6 +6176,189 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct break; } +#ifdef STATIC_LINK_SPD_PLUGIN + case CONTROL_MODE_STABLE_PD: + { + int posVal = body->m_multiBody->getNumPosVars(); + btAlignedObjectArray zeroVel; + int dof = 7 + posVal; + zeroVel.resize(dof); + //clientCmd.m_sendDesiredStateCommandArgument. + //current positions and velocities + btAlignedObjectArray jointPositionsQ; + btAlignedObjectArray jointVelocitiesQdot; + btTransform baseTr = body->m_multiBody->getBaseWorldTransform(); +#if 1 + jointPositionsQ.push_back(baseTr.getOrigin()[0]); + jointPositionsQ.push_back(baseTr.getOrigin()[1]); + jointPositionsQ.push_back(baseTr.getOrigin()[2]); + jointPositionsQ.push_back(baseTr.getRotation()[0]); + jointPositionsQ.push_back(baseTr.getRotation()[1]); + jointPositionsQ.push_back(baseTr.getRotation()[2]); + jointPositionsQ.push_back(baseTr.getRotation()[3]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[0]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[1]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseVel()[2]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[0]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[1]); + jointVelocitiesQdot.push_back(body->m_multiBody->getBaseOmega()[2]); + jointVelocitiesQdot.push_back(0); +#else + for (int i = 0; i < 7; i++) + { + jointPositionsQ.push_back(0); + jointVelocitiesQdot.push_back(0); + } + jointPositionsQ[6] = 1; +#endif + for (int i = 0; i < body->m_multiBody->getNumLinks(); i++) + { + switch (body->m_multiBody->getLink(i).m_jointType) + { + case btMultibodyLink::eSpherical: + { + btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i); + jointPositionsQ.push_back(jointPos[0]); + jointPositionsQ.push_back(jointPos[1]); + jointPositionsQ.push_back(jointPos[2]); + jointPositionsQ.push_back(jointPos[3]); + btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i); + jointVelocitiesQdot.push_back(jointVel[0]); + jointVelocitiesQdot.push_back(jointVel[1]); + jointVelocitiesQdot.push_back(jointVel[2]); + jointVelocitiesQdot.push_back(0); + break; + } + case btMultibodyLink::ePrismatic: + case btMultibodyLink::eRevolute: + { + btScalar* jointPos = body->m_multiBody->getJointPosMultiDof(i); + jointPositionsQ.push_back(jointPos[0]); + btScalar* jointVel = body->m_multiBody->getJointVelMultiDof(i); + jointVelocitiesQdot.push_back(jointVel[0]); + break; + } + case btMultibodyLink::eFixed: + { + //skip + break; + } + default: + { + b3Error("Unsupported joint type"); + btAssert(0); + } + } + } + + + cRBDModel* rbdModel = 0; + + { + BT_PROFILE("findOrCreateRBDModel"); + rbdModel = m_data->findOrCreateRBDModel(body->m_multiBody, &jointPositionsQ[0], &jointVelocitiesQdot[0]); + } + if (rbdModel) + { + + int num_dof = jointPositionsQ.size(); + const Eigen::VectorXd& pose = rbdModel->GetPose(); + const Eigen::VectorXd& vel = rbdModel->GetVel(); + + Eigen::Map mKp((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kp, num_dof); + Eigen::Map mKd((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kd, num_dof); + Eigen::Map maxForce((double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque, num_dof); + + Eigen::DiagonalMatrix Kp_mat = mKp.asDiagonal(); + Eigen::DiagonalMatrix Kd_mat = mKd.asDiagonal(); + + Eigen::MatrixXd M = rbdModel->GetMassMat(); + //rbdModel->UpdateBiasForce(); + const Eigen::VectorXd& C = rbdModel->GetBiasForce(); + M.diagonal() += m_data->m_physicsDeltaTime * mKd; + + Eigen::VectorXd pose_inc; + + const Eigen::MatrixXd& joint_mat = rbdModel->GetJointMat(); + { + BT_PROFILE("cKinTree::VelToPoseDiff"); + cKinTree::VelToPoseDiff(joint_mat, rbdModel->GetPose(), rbdModel->GetVel(), pose_inc); + } + + //tar_pose needs to be reshuffled? + Eigen::VectorXd tar_pose, tar_vel; + + { + BT_PROFILE("convertPose"); + PhysicsServerCommandProcessorInternalData::convertPose(body->m_multiBody, + (double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ, + (double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot, + tar_pose, tar_vel); + } + + pose_inc = pose+m_data->m_physicsDeltaTime * pose_inc; + + { + BT_PROFILE("cKinTree::PostProcessPose"); + cKinTree::PostProcessPose(joint_mat, pose_inc); + } + + Eigen::VectorXd pose_err; + { + BT_PROFILE("cKinTree::CalcVel"); + cKinTree::CalcVel(joint_mat, pose_inc, tar_pose, 1, pose_err); + } + for (int i = 0; i < 7; i++) + { + pose_err[i] = 0; + } + + Eigen::VectorXd vel_err = tar_vel - vel; + Eigen::VectorXd acc; + + { + BT_PROFILE("acc"); + acc = Kp_mat * pose_err + Kd_mat * vel_err - C; + } + + { + BT_PROFILE("M.ldlt().solve"); + acc = M.ldlt().solve(acc); + } + Eigen::VectorXd out_tau = Eigen::VectorXd::Zero(num_dof); + out_tau += Kp_mat * pose_err + Kd_mat * (vel_err - m_data->m_physicsDeltaTime * acc); + //clamp the forces + out_tau = out_tau.cwiseMax(-maxForce); + out_tau = out_tau.cwiseMin(maxForce); + //apply the forces + int torqueIndex = 7; + for (int link = 0; link < mb->getNumLinks(); link++) + { + int dofCount = mb->getLink(link).m_dofCount; + int dofOffset = mb->getLink(link).m_dofOffset; + if (dofCount == 3) + { + for (int dof = 0; dof < 3; dof++) + { + double torque = out_tau[torqueIndex+ dof]; + mb->addJointTorqueMultiDof(link, dof, torque); + } + torqueIndex += 4; + } + if (dofCount == 1) + { + double torque = out_tau[torqueIndex]; + mb->addJointTorqueMultiDof(link, 0, torque); + torqueIndex++; + } + } + + + } + + break; + } +#endif default: { b3Warning("m_controlMode not implemented yet"); @@ -9127,6 +9318,7 @@ bool PhysicsServerCommandProcessor::processConfigureOpenGLVisualizerCommand(cons return hasStatus; } + bool PhysicsServerCommandProcessor::processInverseDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes) { bool hasStatus = true; @@ -9352,7 +9544,7 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru { bool hasStatus = true; BT_PROFILE("CMD_CALCULATE_MASS_MATRIX"); - + SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_CALCULATED_MASS_MATRIX_FAILED; InternalBodyHandle* bodyHandle = m_data->m_bodyHandles.getHandle(clientCmd.m_calculateMassMatrixArguments.m_bodyUniqueId); @@ -9370,8 +9562,9 @@ bool PhysicsServerCommandProcessor::processCalculateMassMatrixCommand(const stru &zeroVel[0]); if (rbdModel) { - Eigen::MatrixXd out_mass; - cRBDUtil::BuildMassMat(*rbdModel, out_mass); + //Eigen::MatrixXd out_mass; + //cRBDUtil::BuildMassMat(*rbdModel, out_mass); + const Eigen::MatrixXd& out_mass = rbdModel->GetMassMat(); int skipDofs = 0;// 7 - baseDofQ; int totDofs = dof; serverCmd.m_massMatrixResultArgs.m_dofCount = totDofs-skipDofs; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 40e54b1d6..56f6657cb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -772,6 +772,7 @@ enum CONTROL_MODE_TORQUE, CONTROL_MODE_POSITION_VELOCITY_PD, CONTROL_MODE_PD, // The standard PD control implemented as soft constraint. + CONTROL_MODE_STABLE_PD, }; ///flags for b3ApplyExternalTorque and b3ApplyExternalForce diff --git a/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp index bc88b38a2..ec38dd02e 100644 --- a/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp +++ b/examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp @@ -1,7 +1,7 @@ #include "RBDModel.h" #include "RBDUtil.h" #include "KinTree.h" - +#include "LinearMath/btQuickprof.h" cRBDModel::cRBDModel() { } @@ -38,11 +38,26 @@ void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel) SetPose(pose); SetVel(vel); - UpdateJointSubspaceArr(); - UpdateChildParentMatArr(); - UpdateSpWorldTrans(); - UpdateMassMat(); - UpdateBiasForce(); + { + BT_PROFILE("rbdModel::UpdateJointSubspaceArr"); + UpdateJointSubspaceArr(); + } + { + BT_PROFILE("rbdModel::UpdateChildParentMatArr"); + UpdateChildParentMatArr(); + } + { + BT_PROFILE("rbdModel::UpdateSpWorldTrans"); + UpdateSpWorldTrans(); + } + { + BT_PROFILE("UpdateMassMat"); + UpdateMassMat(); + } + { + BT_PROFILE("UpdateBiasForce"); + UpdateBiasForce(); + } } int cRBDModel::GetNumDof() const diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index 78d478162..a7eca7604 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -50,6 +50,11 @@ struct btTimings m_firstTiming = false; + if (startTime > endTime) + { + endTime = startTime; + } + unsigned long long int startTimeDiv1000 = startTime / 1000; unsigned long long int endTimeDiv1000 = endTime / 1000; @@ -62,10 +67,7 @@ struct btTimings #else - if (startTime > endTime) - { - endTime = startTime; - } + unsigned int startTimeRem1000 = startTime % 1000; unsigned int endTimeRem1000 = endTime % 1000; diff --git a/examples/pybullet/examples/humanoidMotionCapture.py b/examples/pybullet/examples/humanoidMotionCapture.py index fc3fa9a7c..3c7f8a7a6 100644 --- a/examples/pybullet/examples/humanoidMotionCapture.py +++ b/examples/pybullet/examples/humanoidMotionCapture.py @@ -21,10 +21,10 @@ from pdControllerStable import PDControllerStableMultiDof explicitPD = PDControllerExplicitMultiDof(p) stablePD = PDControllerStableMultiDof(p) -p.resetDebugVisualizerCamera(cameraDistance=7, +p.resetDebugVisualizerCamera(cameraDistance=7.4, cameraYaw=-94, cameraPitch=-14, - cameraTargetPosition=[0.31, 0.03, -1.16]) + cameraTargetPosition=[0.24, -0.02, -0.09]) import pybullet_data p.setTimeOut(10000) @@ -67,7 +67,7 @@ jointTypes = [ "JOINT_REVOLUTE", "JOINT_PRISMATIC", "JOINT_SPHERICAL", "JOINT_PLANAR", "JOINT_FIXED" ] -startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4]] +startLocations = [[0, 0, 2], [0, 0, 0], [0, 0, -2], [0, 0, -4], [0, 0, 4]] p.addUserDebugText("Stable PD", [startLocations[0][0], startLocations[0][1] + 1, startLocations[0][2]], @@ -81,6 +81,9 @@ p.addUserDebugText("Explicit PD", p.addUserDebugText("Kinematic", [startLocations[3][0], startLocations[3][1] + 1, startLocations[3][2]], [0, 0, 0]) +p.addUserDebugText("Stable PD (Py)", + [startLocations[4][0], startLocations[4][1] + 1, startLocations[4][2]], + [0, 0, 0]) humanoid = p.loadURDF("humanoid/humanoid.urdf", startLocations[0], @@ -102,6 +105,11 @@ humanoid4 = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25, useFixedBase=False, flags=p.URDF_MAINTAIN_LINK_ORDER) +humanoid5 = p.loadURDF("humanoid/humanoid.urdf", + startLocations[4], + globalScaling=0.25, + useFixedBase=False, + flags=p.URDF_MAINTAIN_LINK_ORDER) humanoid_fix = p.createConstraint(humanoid, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], startLocations[0], [0, 0, 0, 1]) @@ -111,6 +119,8 @@ humanoid3_fix = p.createConstraint(humanoid3, -1, -1, -1, p.JOINT_FIXED, [0, 0, startLocations[2], [0, 0, 0, 1]) humanoid3_fix = p.createConstraint(humanoid4, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], startLocations[3], [0, 0, 0, 1]) +humanoid4_fix = p.createConstraint(humanoid5, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], + startLocations[4], [0, 0, 0, 1]) startPose = [ 2, 0.847532, 0, 0.9986781045, 0.01410400148, -0.0006980000731, -0.04942300517, 0.9988133229, @@ -135,6 +145,7 @@ p.resetBasePositionAndOrientation(humanoid, startLocations[0], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid2, startLocations[1], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid3, startLocations[2], [0, 0, 0, 1]) p.resetBasePositionAndOrientation(humanoid4, startLocations[3], [0, 0, 0, 1]) +p.resetBasePositionAndOrientation(humanoid5, startLocations[4], [0, 0, 0, 1]) index0 = 7 for j in range(p.getNumJoints(humanoid)): @@ -150,6 +161,7 @@ for j in range(p.getNumJoints(humanoid)): print("spherical position: ", targetPosition) print("spherical velocity: ", targetVel) p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel) + p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel) if (jointType == p.JOINT_PRISMATIC or jointType == p.JOINT_REVOLUTE): targetPosition = [startPose[index0]] @@ -158,6 +170,7 @@ for j in range(p.getNumJoints(humanoid)): print("revolute:", targetPosition) print("revolute velocity:", targetVel) p.resetJointStateMultiDof(humanoid, j, targetValue=targetPosition, targetVelocity=targetVel) + p.resetJointStateMultiDof(humanoid5, j, targetValue=targetPosition, targetVelocity=targetVel) p.resetJointStateMultiDof(humanoid2, j, targetValue=targetPosition, targetVelocity=targetVel) for j in range(p.getNumJoints(humanoid)): @@ -174,6 +187,14 @@ for j in range(p.getNumJoints(humanoid)): positionGain=0, velocityGain=1, force=[0, 0, 0]) + p.setJointMotorControlMultiDof(humanoid5, + j, + p.POSITION_CONTROL, + targetPosition, + targetVelocity=[0, 0, 0], + positionGain=0, + velocityGain=1, + force=[0, 0, 0]) p.setJointMotorControlMultiDof(humanoid3, j, p.POSITION_CONTROL, @@ -195,6 +216,7 @@ for j in range(p.getNumJoints(humanoid)): p.setJointMotorControl2(humanoid, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0) p.setJointMotorControl2(humanoid3, j, p.VELOCITY_CONTROL, targetVelocity=0, force=31) p.setJointMotorControl2(humanoid4, j, p.VELOCITY_CONTROL, targetVelocity=0, force=10) + p.setJointMotorControl2(humanoid5, j, p.VELOCITY_CONTROL, targetVelocity=0, force=0) #print(ji) print("joint[", j, "].type=", jointTypes[ji[2]]) @@ -411,15 +433,42 @@ while (p.isConnected()): #print("pose=") #for po in pose: # print(po) - - taus = stablePD.computePD(bodyUniqueId=humanoid, - jointIndices=jointIndicesAll, - desiredPositions=pose, - desiredVelocities=[0] * totalDofs, - kps=kpOrg, - kds=kdOrg, - maxForces=[maxForce] * totalDofs, - timeStep=timeStep) + + + taus = stablePD.computePD(bodyUniqueId=humanoid5, + jointIndices=jointIndicesAll, + desiredPositions=pose, + desiredVelocities=[0] * totalDofs, + kps=kpOrg, + kds=kdOrg, + maxForces=[maxForce] * totalDofs, + timeStep=timeStep) + + indices = [chest, neck, rightHip, rightKnee, + rightAnkle, rightShoulder, rightElbow, + leftHip, leftKnee, leftAnkle, + leftShoulder, leftElbow] + targetPositions = [chestRot,neckRot,rightHipRot, rightKneeRot, + rightAnkleRot, rightShoulderRot, rightElbowRot, + leftHipRot, leftKneeRot, leftAnkleRot, + leftShoulderRot, leftElbowRot] + maxForces = [ [maxForce,maxForce,maxForce], [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce], + [maxForce,maxForce,maxForce],[maxForce,maxForce,maxForce],[maxForce], + [maxForce,maxForce,maxForce], [maxForce], [maxForce,maxForce,maxForce], + [maxForce,maxForce,maxForce], [maxForce]] + + + kps = [1000]*12 + kds = [100]*12 + + + p.setJointMotorControlMultiDofArray(humanoid, + indices, + p.STABLE_PD_CONTROL, + targetPositions=targetPositions, + positionGains=kps, + velocityGains=kds, + forces=maxForces) taus3 = explicitPD.computePD(bodyUniqueId=humanoid3, jointIndices=jointIndicesAll, @@ -435,9 +484,9 @@ while (p.isConnected()): for index in range(len(jointIndicesAll)): jointIndex = jointIndicesAll[index] if jointDofCounts[index] == 4: - + p.setJointMotorControlMultiDof( - humanoid, + humanoid5, jointIndex, p.TORQUE_CONTROL, force=[taus[dofIndex + 0], taus[dofIndex + 1], taus[dofIndex + 2]]) @@ -449,7 +498,8 @@ while (p.isConnected()): if jointDofCounts[index] == 1: - p.setJointMotorControlMultiDof(humanoid, + + p.setJointMotorControlMultiDof(humanoid5, jointIndex, controlMode=p.TORQUE_CONTROL, force=[taus[dofIndex]]) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py index 1a39a91cb..f5f40fe29 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py @@ -152,58 +152,84 @@ class HumanoidStablePD(object): self.initializePose(self._poseInterpolator, self._kin_model, initBase=False) def initializePose(self, pose, phys_model, initBase, initializeVelocity=True): - + + useArray = True if initializeVelocity: if initBase: self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos, pose._baseOrn) self._pybullet_client.resetBaseVelocity(phys_model, pose._baseLinVel, pose._baseAngVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, - pose._chestVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, pose._neckVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot, - pose._rightHipVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot, - pose._rightKneeVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot, - pose._rightAnkleVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder, - pose._rightShoulderRot, pose._rightShoulderVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot, - pose._rightElbowVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot, - pose._leftHipVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot, - pose._leftKneeVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot, - pose._leftAnkleVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder, - pose._leftShoulderRot, pose._leftShoulderVel) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot, - pose._leftElbowVel) + if useArray: + indices = [chest,neck,rightHip,rightKnee, + rightAnkle, rightShoulder, rightElbow,leftHip, + leftKnee, leftAnkle, leftShoulder,leftElbow] + jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot, + pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot, + pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot] + + jointVelocities = [pose._chestVel, pose._neckVel, pose._rightHipVel, pose._rightKneeVel, + pose._rightAnkleVel, pose._rightShoulderVel, pose._rightElbowVel, pose._leftHipVel, + pose._leftKneeVel, pose._leftAnkleVel, pose._leftShoulderVel, pose._leftElbowVel] + self._pybullet_client.resetJointStatesMultiDof(phys_model, indices, + jointPositions, jointVelocities) + else: + self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, + pose._chestVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, pose._neckVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot, + pose._rightHipVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot, + pose._rightKneeVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot, + pose._rightAnkleVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder, + pose._rightShoulderRot, pose._rightShoulderVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot, + pose._rightElbowVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot, + pose._leftHipVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot, + pose._leftKneeVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot, + pose._leftAnkleVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder, + pose._leftShoulderRot, pose._leftShoulderVel) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot, + pose._leftElbowVel) else: + if initBase: self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos, pose._baseOrn) - self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot, - [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot, [0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot, - [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder, - pose._rightShoulderRot, [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot, - [0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot, - [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot, [0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot, - [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder, - pose._leftShoulderRot, [0, 0, 0]) - self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot, [0]) + if useArray: + indices = [chest,neck,rightHip,rightKnee, + rightAnkle, rightShoulder, rightElbow,leftHip, + leftKnee, leftAnkle, leftShoulder,leftElbow] + jointPositions = [pose._chestRot, pose._neckRot, pose._rightHipRot, pose._rightKneeRot, + pose._rightAnkleRot, pose._rightShoulderRot, pose._rightElbowRot, pose._leftHipRot, + pose._leftKneeRot, pose._leftAnkleRot, pose._leftShoulderRot, pose._leftElbowRot] + self._pybullet_client.resetJointStatesMultiDof(phys_model, indices,jointPositions) + + else: + self._pybullet_client.resetJointStateMultiDof(phys_model, chest, pose._chestRot, [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, neck, pose._neckRot, [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightHip, pose._rightHipRot, + [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightKnee, pose._rightKneeRot, [0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightAnkle, pose._rightAnkleRot, + [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightShoulder, + pose._rightShoulderRot, [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, rightElbow, pose._rightElbowRot, + [0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftHip, pose._leftHipRot, + [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftKnee, pose._leftKneeRot, [0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftAnkle, pose._leftAnkleRot, + [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftShoulder, + pose._leftShoulderRot, [0, 0, 0]) + self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot, [0]) def calcCycleCount(self, simTime, cycleTime): phases = simTime / cycleTime @@ -275,6 +301,57 @@ class HumanoidStablePD(object): pose = self._poseInterpolator.ConvertFromAction(self._pybullet_client, action) return pose + def computeAndApplyPDForces(self, desiredPositions, maxForces): + dofIndex = 7 + scaling = 1 + indices = [] + forces = [] + targetPositions=[] + targetVelocities=[] + kps = [] + kds = [] + + for index in range(len(self._jointIndicesAll)): + jointIndex = self._jointIndicesAll[index] + indices.append(jointIndex) + kps.append(self._kpOrg[dofIndex]) + kds.append(self._kdOrg[dofIndex]) + if self._jointDofCounts[index] == 4: + force = [ + scaling * maxForces[dofIndex + 0], + scaling * maxForces[dofIndex + 1], + scaling * maxForces[dofIndex + 2] + ] + targetVelocity = [0,0,0] + targetPosition = [ + desiredPositions[dofIndex + 0], + desiredPositions[dofIndex + 1], + desiredPositions[dofIndex + 2], + desiredPositions[dofIndex + 3] + ] + if self._jointDofCounts[index] == 1: + force = [scaling * maxForces[dofIndex]] + targetPosition = [desiredPositions[dofIndex+0]] + targetVelocity = [0] + forces.append(force) + targetPositions.append(targetPosition) + targetVelocities.append(targetVelocity) + dofIndex += self._jointDofCounts[index] + + #static char* kwlist[] = { "bodyUniqueId", + #"jointIndices", + #"controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "maxVelocities", "physicsClientId", NULL }; + + self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model, + indices, + self._pybullet_client.STABLE_PD_CONTROL, + targetPositions = targetPositions, + targetVelocities = targetVelocities, + forces=forces, + positionGains = kps, + velocityGains = kds, + ) + def computePDForces(self, desiredPositions, desiredVelocities, maxForces): if desiredVelocities == None: desiredVelocities = [0] * self._totalDofs @@ -292,27 +369,50 @@ class HumanoidStablePD(object): def applyPDForces(self, taus): dofIndex = 7 scaling = 1 - for index in range(len(self._jointIndicesAll)): - jointIndex = self._jointIndicesAll[index] - if self._jointDofCounts[index] == 4: - force = [ - scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1], - scaling * taus[dofIndex + 2] - ] - #print("force[", jointIndex,"]=",force) - self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, - jointIndex, - self._pybullet_client.TORQUE_CONTROL, - force=force) - if self._jointDofCounts[index] == 1: - force = [scaling * taus[dofIndex]] - #print("force[", jointIndex,"]=",force) - self._pybullet_client.setJointMotorControlMultiDof( - self._sim_model, - jointIndex, - controlMode=self._pybullet_client.TORQUE_CONTROL, - force=force) - dofIndex += self._jointDofCounts[index] + useArray = True + indices = [] + forces = [] + + if (useArray): + for index in range(len(self._jointIndicesAll)): + jointIndex = self._jointIndicesAll[index] + indices.append(jointIndex) + if self._jointDofCounts[index] == 4: + force = [ + scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1], + scaling * taus[dofIndex + 2] + ] + if self._jointDofCounts[index] == 1: + force = [scaling * taus[dofIndex]] + #print("force[", jointIndex,"]=",force) + forces.append(force) + dofIndex += self._jointDofCounts[index] + self._pybullet_client.setJointMotorControlMultiDofArray(self._sim_model, + indices, + self._pybullet_client.TORQUE_CONTROL, + forces=forces) + else: + for index in range(len(self._jointIndicesAll)): + jointIndex = self._jointIndicesAll[index] + if self._jointDofCounts[index] == 4: + force = [ + scaling * taus[dofIndex + 0], scaling * taus[dofIndex + 1], + scaling * taus[dofIndex + 2] + ] + #print("force[", jointIndex,"]=",force) + self._pybullet_client.setJointMotorControlMultiDof(self._sim_model, + jointIndex, + self._pybullet_client.TORQUE_CONTROL, + force=force) + if self._jointDofCounts[index] == 1: + force = [scaling * taus[dofIndex]] + #print("force[", jointIndex,"]=",force) + self._pybullet_client.setJointMotorControlMultiDof( + self._sim_model, + jointIndex, + controlMode=self._pybullet_client.TORQUE_CONTROL, + force=force) + dofIndex += self._jointDofCounts[index] def setJointMotors(self, desiredPositions, maxForces): controlMode = self._pybullet_client.POSITION_CONTROL @@ -531,10 +631,17 @@ class HumanoidStablePD(object): #self.pb2dmJoints=[0,1,2,9,10,11,3,4,5,12,13,14,6,7,8] self.pb2dmJoints = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14] + linkIndicesSim = [] + for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)): + linkIndicesSim.append(self.pb2dmJoints[pbJoint]) + + linkStatesSim = self._pybullet_client.getLinkStates(self._sim_model, linkIndicesSim, computeForwardKinematics=True, computeLinkVelocity=True) + for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)): j = self.pb2dmJoints[pbJoint] #print("joint order:",j) - ls = self._pybullet_client.getLinkState(self._sim_model, j, computeForwardKinematics=True) + #ls = self._pybullet_client.getLinkState(self._sim_model, j, computeForwardKinematics=True) + ls = linkStatesSim[pbJoint] linkPos = ls[0] linkOrn = ls[1] linkPosLocal, linkOrnLocal = self._pybullet_client.multiplyTransforms( @@ -560,9 +667,12 @@ class HumanoidStablePD(object): stateVector.append(linkOrnLocal[1]) stateVector.append(linkOrnLocal[2]) + for pbJoint in range(self._pybullet_client.getNumJoints(self._sim_model)): j = self.pb2dmJoints[pbJoint] - ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True) + #ls = self._pybullet_client.getLinkState(self._sim_model, j, computeLinkVelocity=True) + ls = linkStatesSim[pbJoint] + linkLinVel = ls[6] linkAngVel = ls[7] linkLinVelLocal, unused = self._pybullet_client.multiplyTransforms([0, 0, 0], rootTransOrn, @@ -706,16 +816,27 @@ class HumanoidStablePD(object): root_ang_vel_err = self.calcRootAngVelErr(angVelSim, angVelKin) vel_err += root_rot_w * root_ang_vel_err + useArray = False + + if useArray: + jointIndices = range(num_joints) + simJointStates = self._pybullet_client.getJointStatesMultiDof(self._sim_model, jointIndices) + kinJointStates = self._pybullet_client.getJointStatesMultiDof(self._kin_model, jointIndices) for j in range(num_joints): curr_pose_err = 0 curr_vel_err = 0 w = mJointWeights[j] - - simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j) + if useArray: + simJointInfo = simJointStates[j] + else: + simJointInfo = self._pybullet_client.getJointStateMultiDof(self._sim_model, j) #print("simJointInfo.pos=",simJointInfo[0]) #print("simJointInfo.vel=",simJointInfo[1]) - kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_model, j) + if useArray: + kinJointInfo = kinJointStates[j] + else: + kinJointInfo = self._pybullet_client.getJointStateMultiDof(self._kin_model, j) #print("kinJointInfo.pos=",kinJointInfo[0]) #print("kinJointInfo.vel=",kinJointInfo[1]) if (len(simJointInfo[0]) == 1): diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py index 764d70c3d..d0b0bd11b 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py @@ -52,7 +52,7 @@ class PyBulletDeepMimicEnv(Env): motionPath = pybullet_data.getDataPath() + "/" + motion_file[0] #motionPath = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt" self._mocapData.Load(motionPath) - timeStep = 1. / 600. + timeStep = 1. / 240. useFixedBase = False self._humanoid = humanoid_stable_pd.HumanoidStablePD(self._pybullet_client, self._mocapData, timeStep, useFixedBase) @@ -283,10 +283,16 @@ class PyBulletDeepMimicEnv(Env): ] if self._useStablePD: - taus = self._humanoid.computePDForces(self.desiredPose, + usePythonStablePD = False + if usePythonStablePD: + taus = self._humanoid.computePDForces(self.desiredPose, desiredVelocities=None, maxForces=maxForces) - self._humanoid.applyPDForces(taus) + #taus = [0]*43 + self._humanoid.applyPDForces(taus) + else: + self._humanoid.computeAndApplyPDForces(self.desiredPose, + maxForces=maxForces) else: self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces) diff --git a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py index c689a062a..7f52bf08d 100644 --- a/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py +++ b/examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py @@ -47,7 +47,7 @@ def isKeyTriggered(keys, key): animating = False singleStep = False - +humanoid.resetPose() t = 0 while (1): @@ -66,7 +66,7 @@ while (1): #print("t=",t) for i in range(1): - print("t=", t) + #print("t=", t) humanoid.setSimTime(t) humanoid.computePose(humanoid._frameFraction) @@ -75,9 +75,11 @@ while (1): #humanoid.resetPose() desiredPose = humanoid.computePose(humanoid._frameFraction) + #desiredPose = desiredPose.GetPose() #curPose = HumanoidPoseInterpolator() #curPose.reset() + s = humanoid.getState() #np.savetxt("pb_record_state_s.csv", s, delimiter=",") maxForces = [ @@ -85,10 +87,14 @@ while (1): 90, 90, 100, 100, 100, 100, 60, 200, 200, 200, 200, 150, 90, 90, 90, 90, 100, 100, 100, 100, 60 ] - taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces) - - #print("taus=",taus) - humanoid.applyPDForces(taus) + + usePythonStablePD = False + if usePythonStablePD: + taus = humanoid.computePDForces(desiredPose, desiredVelocities=None, maxForces=maxForces) + #Print("taus=",taus) + humanoid.applyPDForces(taus) + else: + humanoid.computeAndApplyPDForces(desiredPose,maxForces=maxForces) pybullet_client.stepSimulation() t += 1. / 600. diff --git a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py index 58854027b..a6b30c152 100644 --- a/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py +++ b/examples/pybullet/gym/pybullet_utils/pd_controller_stable.py @@ -62,8 +62,15 @@ class PDControllerStableMultiDof(object): qIndex = 7 qdotIndex = 7 zeroAccelerations = [0, 0, 0, 0, 0, 0, 0] + useArray = True + if useArray: + jointStates = self._pb.getJointStatesMultiDof(bodyUniqueId,jointIndices) + for i in range(numJoints): - js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i]) + if useArray: + js = jointStates[i] + else: + js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i]) jointPos = js[0] jointVel = js[1] @@ -153,8 +160,6 @@ class PDControllerStableMultiDof(object): if useNumpySolver: qddot = np.linalg.solve(A, b) else: - dofCount = len(b) - print(dofCount) qddot = self._pb.ldltSolve(bodyUniqueId, jointPositions=q1, b=b.tolist(), kd=kds, t=timeStep) tau = p + d - Kd.dot(qddot) * timeStep diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 00d722ee6..28be6a25d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2569,6 +2569,399 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar // return NULL; } + + +static PyObject* pybullet_setJointMotorControlMultiDofArray(PyObject* self, PyObject* args, PyObject* keywds) +{ + int bodyUniqueId, controlMode; + + + PyObject* jointIndicesObj = 0; + PyObject* targetPositionsObj = 0; + PyObject* targetVelocitiesObj = 0; + PyObject* forcesObj = 0; + PyObject* kpsObj = 0; + PyObject* kdsObj = 0; + PyObject* maxVelocitiesObj = 0; + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "jointIndices", "controlMode", "targetPositions", "targetVelocities", "forces", "positionGains", "velocityGains", "maxVelocities", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOi|OOOOOOi", kwlist, &bodyUniqueId, &jointIndicesObj, &controlMode, + &targetPositionsObj, &targetVelocitiesObj, &forcesObj, &kpsObj, &kdsObj, &maxVelocitiesObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryStatusHandle statusHandle; + int numJoints = 0; + int j = 0; + int numControlledDofs = 0; + int numKps = 0; + int numKds = 0; + int numTargetPositionObjs = 0; + int numTargetVelocityobjs = 0; + int numForceObj = 0; + + PyObject* jointIndicesSeq = 0; + PyObject* targetPositionsSeq = 0; + PyObject* targetVelocitiesSeq = 0; + PyObject* forcesSeq = 0; + PyObject* kpsSeq = 0; + PyObject* kdsSeq = 0; + + b3SharedMemoryCommandHandle commandHandle; + commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, controlMode); + + numJoints = b3GetNumJoints(sm, bodyUniqueId); + + if ((controlMode != CONTROL_MODE_TORQUE) && + (controlMode != CONTROL_MODE_PD) && + (controlMode != CONTROL_MODE_STABLE_PD) && + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + { + PyErr_SetString(SpamError, "Illegal control mode."); + return NULL; + } + + jointIndicesSeq = PySequence_Fast(jointIndicesObj, "expected a sequence of joint indices"); + + if (jointIndicesSeq == 0) + { + PyErr_SetString(SpamError, "expected a sequence of joint indices"); + return NULL; + } + + numControlledDofs = jointIndicesObj ? PySequence_Size(jointIndicesObj) : 0; + numKps = kpsObj? PySequence_Size(kpsObj) : 0; + numKds = kdsObj ? PySequence_Size(kdsObj):0; + numTargetPositionObjs = targetPositionsObj?PySequence_Size(targetPositionsObj):0; + numTargetVelocityobjs = targetVelocitiesObj?PySequence_Size(targetVelocitiesObj):0; + numForceObj = forcesObj?PySequence_Size(forcesObj):0; + + if ((numControlledDofs == 0) || + ((numKps>0) && (numControlledDofs != numKps)) || + ((numKds>0) && (numControlledDofs != numKds)) || + ((numTargetPositionObjs>0) && (numControlledDofs != numTargetPositionObjs)) || + ((numTargetVelocityobjs>0) && (numControlledDofs != numTargetVelocityobjs)) || + ((numForceObj>0) && (numControlledDofs != numForceObj)) + ) + { + Py_DECREF(jointIndicesSeq); + Py_INCREF(Py_None); + return Py_None; + } + + if (targetPositionsObj) + { + targetPositionsSeq = PySequence_Fast(targetPositionsObj, "expected a targetPositions sequence"); + } + if (targetVelocitiesObj) + { + targetVelocitiesSeq = PySequence_Fast(targetVelocitiesObj, "expected a targetVelocities sequence"); + } + if (forcesObj) + { + forcesSeq = PySequence_Fast(forcesObj, "expected a forces sequence"); + } + if (kpsObj) + { + kpsSeq = PySequence_Fast(kpsObj, "expected a kps sequence"); + } + if (kdsObj) + { + kdsSeq = PySequence_Fast(kdsObj, "expected a kds sequence"); + } + + + + for (j = 0; j < numControlledDofs; j++) + { + double targetPositionArray[4] = { 0, 0, 0, 1 }; + double targetVelocityArray[4] = { 0, 0, 0 , 0}; + double targetForceArray[4] = { 100000.0, 100000.0, 100000.0 ,0}; + int targetPositionSize = 0; + int targetVelocitySize = 0; + int targetForceSize = 0; + PyObject* targetPositionObj = 0; + PyObject* targetVelocityObj = 0; + PyObject* targetForceObj = 0; + + double kp = 0.1; + double kd = 1.0; + double maxVelocity = -1; + int numTargetPositions = -1; + int jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, j); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + Py_DECREF(jointIndicesSeq); + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + if (numTargetPositionObjs > 0) + { + targetPositionObj = PyList_GET_ITEM(targetPositionsSeq, j); + } + if (numTargetVelocityobjs > 0) + { + targetVelocityObj = PyList_GET_ITEM(targetVelocitiesSeq, j); + } + if (numForceObj > 0) + { + targetForceObj = PyList_GET_ITEM(forcesSeq, j); + } + if (numKps > 0) + { + kp = pybullet_internalGetFloatFromSequence(kpsSeq, j); + } + if (numKds>0) + { + kd = pybullet_internalGetFloatFromSequence(kdsSeq, j); + } + + if (targetPositionObj) + { + PyObject* targetPositionSeq = 0; + int i = 0; + targetPositionSeq = PySequence_Fast(targetPositionObj, "expected a targetPosition sequence"); + targetPositionSize = PySequence_Size(targetPositionObj); + + if (targetPositionSize < 0) + { + targetPositionSize = 0; + } + if (targetPositionSize > 4) + { + targetPositionSize = 4; + } + if (targetPositionSeq) + { + for (i = 0; i < targetPositionSize; i++) + { + targetPositionArray[i] = pybullet_internalGetFloatFromSequence(targetPositionSeq, i); + } + Py_DECREF(targetPositionSeq); + targetPositionSeq = 0; + } + } + + + if (targetVelocityObj) + { + int i = 0; + PyObject* targetVelocitySeq = 0; + targetVelocitySeq = PySequence_Fast(targetVelocityObj, "expected a targetVelocity sequence"); + targetVelocitySize = PySequence_Size(targetVelocityObj); + + if (targetVelocitySize < 0) + { + targetVelocitySize = 0; + } + if (targetVelocitySize > 3) + { + targetVelocitySize = 3; + } + if (targetVelocitySeq) + { + for (i = 0; i < targetVelocitySize; i++) + { + targetVelocityArray[i] = pybullet_internalGetFloatFromSequence(targetVelocitySeq, i); + } + Py_DECREF(targetVelocitySeq); + targetVelocitySeq = 0; + } + } + + + + if (targetForceObj) + { + int i = 0; + PyObject* targetForceSeq = 0; + targetForceSeq = PySequence_Fast(targetForceObj, "expected a force sequence"); + targetForceSize = PySequence_Size(targetForceObj); + + if (targetForceSize < 0) + { + targetForceSize = 0; + } + if (targetForceSize > 3) + { + targetForceSize = 3; + } + if (targetForceSeq) + { + for (i = 0; i < targetForceSize; i++) + { + targetForceArray[i] = pybullet_internalGetFloatFromSequence(targetForceSeq, i); + } + Py_DECREF(targetForceSeq); + targetForceSeq = 0; + } + } + + //if (targetPositionSize == 0 && targetVelocitySize == 0) + //{ + + { + + struct b3JointInfo info; + + + b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info); + + switch (controlMode) + { + case CONTROL_MODE_TORQUE: + { + if (info.m_uSize == targetForceSize) + { + b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex, + targetForceArray, targetForceSize); + } + break; + } + case CONTROL_MODE_STABLE_PD: + case CONTROL_MODE_POSITION_VELOCITY_PD: + case CONTROL_MODE_PD: + { + //make sure size == info.m_qSize + + if (maxVelocity > 0) + { + b3JointControlSetMaximumVelocity(commandHandle, info.m_uIndex, maxVelocity); + } + + if (info.m_qSize == targetPositionSize) + { + b3JointControlSetDesiredPositionMultiDof(commandHandle, info.m_qIndex, + targetPositionArray, targetPositionSize); + } + else + { + //printf("Warning: targetPosition array size doesn't match joint position size (got %d, expected %d).",targetPositionSize, info.m_qSize); + } + + if (controlMode == CONTROL_MODE_STABLE_PD) + { + if (targetVelocitySize == 0) + { + targetVelocitySize = info.m_uSize; + targetVelocityArray[0] = 0; + targetVelocityArray[1] = 0; + targetVelocityArray[2] = 0; + targetVelocityArray[3] = 0; + } + if (info.m_uSize == 3) + { + b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_qIndex, + targetVelocityArray, targetVelocitySize + 1); + } + else + { + b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_qIndex, + targetVelocityArray, targetVelocitySize); + } + } + else + { + if (info.m_uSize == targetVelocitySize) + { + b3JointControlSetDesiredVelocityMultiDof(commandHandle, info.m_uIndex, + targetVelocityArray, targetVelocitySize); + } + } + + + + if (controlMode == CONTROL_MODE_STABLE_PD) + { + if (info.m_uSize == 3) + { + b3JointControlSetKp(commandHandle, info.m_qIndex + 0, kp); + b3JointControlSetKp(commandHandle, info.m_qIndex + 1, kp); + b3JointControlSetKp(commandHandle, info.m_qIndex + 2, kp); + b3JointControlSetKp(commandHandle, info.m_qIndex + 3, kp); + + b3JointControlSetKd(commandHandle, info.m_qIndex + 0, kd); + b3JointControlSetKd(commandHandle, info.m_qIndex + 1, kd); + b3JointControlSetKd(commandHandle, info.m_qIndex + 2, kd); + b3JointControlSetKd(commandHandle, info.m_qIndex + 3, kd); + + b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_qIndex, + targetForceArray, targetForceSize+1); + } + else + { + b3JointControlSetKp(commandHandle, info.m_qIndex, kp); + b3JointControlSetKd(commandHandle, info.m_qIndex, kd); + b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_qIndex, + targetForceArray, targetForceSize); + } + } + else + { + b3JointControlSetKp(commandHandle, info.m_uIndex, kp); + b3JointControlSetKd(commandHandle, info.m_uIndex, kd); + if (info.m_uSize == targetForceSize || targetForceSize == 1) + { + b3JointControlSetDesiredForceTorqueMultiDof(commandHandle, info.m_uIndex, + targetForceArray, targetForceSize); + } + } + + break; + } + default: + { + } + }; + } + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + + if (jointIndicesSeq) + { + Py_DECREF(jointIndicesSeq); + } + if (targetPositionsSeq) + { + Py_DECREF(targetPositionsSeq); + } + if (targetVelocitiesSeq) + { + Py_DECREF(targetVelocitiesSeq); + } + if (forcesSeq) + { + Py_DECREF(forcesSeq); + } + if (kpsSeq) + { + Py_DECREF(kpsSeq); + } + if (kdsSeq) + { + Py_DECREF(kdsSeq); + } + + Py_INCREF(Py_None); + return Py_None; + } + // PyErr_SetString(SpamError, "Error parsing arguments in setJointControl."); + // return NULL; +} + + static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId, jointIndex, controlMode; @@ -2781,6 +3174,7 @@ static PyObject* pybullet_setJointMotorControlMultiDof(PyObject* self, PyObject* // return NULL; } + static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, PyObject* keywds) { int bodyUniqueId, jointIndex, controlMode; @@ -3844,6 +4238,181 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args, PyObje return Py_None; } + +// Initalize all joint positions given a list of values +static PyObject* pybullet_resetJointStatesMultiDof(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + b3PhysicsClientHandle sm = 0; + int bodyUniqueId; + PyObject* jointIndicesObj = 0; + PyObject* targetPositionsObj = 0; + PyObject* targetVelocitiesObj = 0; + PyObject* jointIndicesSeq = 0; + int numIndices = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "jointIndices", "targetValues", "targetVelocities", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|Oi", kwlist, &bodyUniqueId, &jointIndicesObj, &targetPositionsObj, &targetVelocitiesObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + numIndices = PySequence_Size(jointIndicesObj); + if (numIndices == 0) + { + Py_INCREF(Py_None); + return Py_None; + } + + jointIndicesSeq = PySequence_Fast(jointIndicesObj, "expected a sequence of joint indices"); + + if (jointIndicesSeq == 0) + { + PyErr_SetString(SpamError, "expected a sequence of joint indices"); + return NULL; + } + + { + int i; + int numJoints; + int numTargetPositionObjs, numTargetVelocityobjs; + PyObject* targetPositionsSeq = 0; + PyObject* targetVelocitiesSeq = 0; + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); + numJoints = b3GetNumJoints(sm, bodyUniqueId); + + numTargetPositionObjs = targetPositionsObj ? PySequence_Size(targetPositionsObj) : 0; + numTargetVelocityobjs = targetVelocitiesObj ? PySequence_Size(targetVelocitiesObj) : 0; + + + if ( + ((numTargetPositionObjs > 0) && (numIndices != numTargetPositionObjs)) || + ((numTargetVelocityobjs > 0) && (numIndices != numTargetVelocityobjs)) + ) + { + Py_DECREF(jointIndicesSeq); + } + + + targetPositionsSeq = PySequence_Fast(targetPositionsObj, "expected a sequence of target positions"); + if (targetVelocitiesObj) + targetVelocitiesSeq = PySequence_Fast(targetVelocitiesObj, "expected a sequence of target positions"); + for (i = 0; i < numIndices; i++) + { + + int jointIndex = pybullet_internalGetIntFromSequence(jointIndicesSeq, i); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + Py_DECREF(jointIndicesSeq); + PyErr_SetString(SpamError, "Joint index out-of-range."); + return NULL; + } + + + double targetPositionArray[4] = { 0, 0, 0, 1 }; + double targetVelocityArray[3] = { 0, 0, 0 }; + int targetPositionSize = 0; + int targetVelocitySize = 0; + PyObject* targetPositionObj = 0; + PyObject* targetVelocityObj = 0; + + + if (numTargetPositionObjs > 0) + { + targetPositionObj = PyList_GET_ITEM(targetPositionsSeq, i); + } + if (numTargetVelocityobjs > 0) + { + targetVelocityObj = PyList_GET_ITEM(targetVelocitiesSeq, i); + } + + + if (targetPositionObj) + { + PyObject* targetPositionSeq = 0; + int i = 0; + targetPositionSeq = PySequence_Fast(targetPositionObj, "expected a targetPosition sequence"); + targetPositionSize = PySequence_Size(targetPositionObj); + + if (targetPositionSize < 0) + { + targetPositionSize = 0; + } + if (targetPositionSize > 4) + { + targetPositionSize = 4; + } + if (targetPositionSeq) + { + for (i = 0; i < targetPositionSize; i++) + { + targetPositionArray[i] = pybullet_internalGetFloatFromSequence(targetPositionSeq, i); + } + Py_DECREF(targetPositionSeq); + } + } + + if (targetVelocityObj) + { + int i = 0; + PyObject* targetVelocitySeq = 0; + targetVelocitySeq = PySequence_Fast(targetVelocityObj, "expected a targetVelocity sequence"); + targetVelocitySize = PySequence_Size(targetVelocityObj); + + if (targetVelocitySize < 0) + { + targetVelocitySize = 0; + } + if (targetVelocitySize > 3) + { + targetVelocitySize = 3; + } + if (targetVelocitySeq) + { + for (i = 0; i < targetVelocitySize; i++) + { + targetVelocityArray[i] = pybullet_internalGetFloatFromSequence(targetVelocitySeq, i); + } + Py_DECREF(targetVelocitySeq); + } + } + + if (targetPositionSize == 0 && targetVelocitySize == 0) + { + PyErr_SetString(SpamError, "Expected an position and/or velocity list."); + return NULL; + } + { + + + if (targetPositionSize) + { + b3CreatePoseCommandSetJointPositionMultiDof(sm, commandHandle, jointIndex, targetPositionArray, targetPositionSize); + } + if (targetVelocitySize) + { + b3CreatePoseCommandSetJointVelocityMultiDof(sm, commandHandle, jointIndex, targetVelocityArray, targetVelocitySize); + } + } + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_DECREF(jointIndicesSeq); + } + } + Py_INCREF(Py_None); + return Py_None; +} + // Initalize all joint positions given a list of values static PyObject* pybullet_resetJointStateMultiDof(PyObject* self, PyObject* args, PyObject* keywds) { @@ -4447,6 +5016,148 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args, return Py_None; } +static PyObject* pybullet_getJointStatesMultiDof(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyListPosition; + PyObject* pyListVelocity; + PyObject* pyListJointMotorTorque; + + struct b3JointSensorState2 sensorState; + + int bodyUniqueId = -1; + + PyObject* jointIndicesObj = 0; + int sensorStateSize = 4; // size of struct b3JointSensorState + int forceTorqueSize = 6; // size of force torque list from b3JointSensorState + int j; + + b3PhysicsClientHandle sm = 0; + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "jointIndex", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|i", kwlist, &bodyUniqueId, &jointIndicesObj, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getJointState failed; invalid bodyUniqueId"); + + return NULL; + } + + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getJointState failed."); + + return NULL; + } + + { + int numJoints, numRequestedJoints; + int jnt; + PyObject* jointIndicesSeq = 0; + PyObject* resultListJointState = 0; + + numJoints = b3GetNumJoints(sm, bodyUniqueId); + jointIndicesSeq = PySequence_Fast(jointIndicesObj, "expected a sequence of joint indices"); + + if (jointIndicesSeq == 0) + { + PyErr_SetString(SpamError, "expected a sequence of joint indices"); + return NULL; + } + + numRequestedJoints = PySequence_Size(jointIndicesObj); + if (numRequestedJoints == 0) + { + Py_DECREF(jointIndicesSeq); + Py_INCREF(Py_None); + return Py_None; + } + + resultListJointState = PyTuple_New(numRequestedJoints); + + for (jnt = 0; jnt < numRequestedJoints; jnt++) + { + PyObject* pyListJointForceTorque; + PyObject* pyListJointState; + int jointIndex = pybullet_internalGetFloatFromSequence(jointIndicesSeq, jnt); + + pyListJointState = PyTuple_New(sensorStateSize); + pyListJointForceTorque = PyTuple_New(forceTorqueSize); + + if (b3GetJointStateMultiDof(sm, status_handle, jointIndex, &sensorState)) + { + int i = 0; + pyListPosition = PyTuple_New(sensorState.m_qDofSize); + pyListVelocity = PyTuple_New(sensorState.m_uDofSize); + pyListJointMotorTorque = PyTuple_New(sensorState.m_uDofSize); + PyTuple_SetItem(pyListJointState, 0, pyListPosition); + PyTuple_SetItem(pyListJointState, 1, pyListVelocity); + + for (i = 0; i < sensorState.m_qDofSize; i++) + { + PyTuple_SetItem(pyListPosition, i, + PyFloat_FromDouble(sensorState.m_jointPosition[i])); + } + + for (i = 0; i < sensorState.m_uDofSize; i++) + { + PyTuple_SetItem(pyListVelocity, i, + PyFloat_FromDouble(sensorState.m_jointVelocity[i])); + + PyTuple_SetItem(pyListJointMotorTorque, i, + PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i])); + } + + for (j = 0; j < forceTorqueSize; j++) + { + PyTuple_SetItem(pyListJointForceTorque, j, + PyFloat_FromDouble(sensorState.m_jointReactionForceTorque[j])); + } + + PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque); + + PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque); + + PyTuple_SetItem(resultListJointState, jnt, pyListJointState); + } + else + { + PyErr_SetString(SpamError, "getJointState failed (2)."); + Py_DECREF(jointIndicesSeq); + return NULL; + } + } + Py_DECREF(jointIndicesSeq); + return resultListJointState; + } + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + static PyObject* pybullet_getJointStates(PyObject* self, PyObject* args, PyObject* keywds) { PyObject* pyListJointForceTorque; @@ -4726,6 +5437,191 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* return Py_None; } +static PyObject* pybullet_getLinkStates(PyObject* self, PyObject* args, PyObject* keywds) +{ + PyObject* pyLinkState; + PyObject* pyLinkStateWorldPosition; + PyObject* pyLinkStateWorldOrientation; + PyObject* pyLinkStateLocalInertialPosition; + PyObject* pyLinkStateLocalInertialOrientation; + PyObject* pyLinkStateWorldLinkFramePosition; + PyObject* pyLinkStateWorldLinkFrameOrientation; + PyObject* pyLinkStateWorldLinkLinearVelocity; + PyObject* pyLinkStateWorldLinkAngularVelocity; + PyObject* linkIndicesObj = 0; + + struct b3LinkState linkState; + + int bodyUniqueId = -1; + + int computeLinkVelocity = 0; + int computeForwardKinematics = 0; + + + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "bodyUniqueId", "linkIndices", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iO|iii", kwlist, &bodyUniqueId, &linkIndicesObj, &computeLinkVelocity, &computeForwardKinematics, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + { + int status_type = 0; + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle status_handle; + PyObject* linkIndicesSeq = 0; + int numRequestedLinks = -1; + int numJoints = 0; + int link = -1; + PyObject* resultListLinkState = 0; + if (bodyUniqueId < 0) + { + PyErr_SetString(SpamError, "getLinkState failed; invalid bodyUniqueId"); + return NULL; + } + + cmd_handle = + b3RequestActualStateCommandInit(sm, bodyUniqueId); + + if (computeLinkVelocity) + { + b3RequestActualStateCommandComputeLinkVelocity(cmd_handle, computeLinkVelocity); + } + + if (computeForwardKinematics) + { + b3RequestActualStateCommandComputeForwardKinematics(cmd_handle, computeForwardKinematics); + } + + status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + status_type = b3GetStatusType(status_handle); + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + PyErr_SetString(SpamError, "getLinkState failed."); + return NULL; + } + + linkIndicesSeq = PySequence_Fast(linkIndicesObj, "expected a sequence of link indices"); + + if (linkIndicesSeq == 0) + { + PyErr_SetString(SpamError, "expected a sequence of joint indices"); + return NULL; + } + + numRequestedLinks = PySequence_Size(linkIndicesObj); + numJoints = b3GetNumJoints(sm, bodyUniqueId); + resultListLinkState = PyTuple_New(numRequestedLinks); + for (link=0;link= 0)) + { + if (b3GetLinkState(sm, status_handle, linkIndex, &linkState)) + { + int i; + pyLinkStateWorldPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldPosition, i, + PyFloat_FromDouble(linkState.m_worldPosition[i])); + } + + pyLinkStateWorldOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldOrientation, i, + PyFloat_FromDouble(linkState.m_worldOrientation[i])); + } + + pyLinkStateLocalInertialPosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialPosition, i, + PyFloat_FromDouble(linkState.m_localInertialPosition[i])); + } + + pyLinkStateLocalInertialOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateLocalInertialOrientation, i, + PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); + } + + pyLinkStateWorldLinkFramePosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFramePosition, i, + PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); + } + + pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, + PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); + } + + if (computeLinkVelocity) + { + pyLinkState = PyTuple_New(8); + } + else + { + pyLinkState = PyTuple_New(6); + } + + PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); + PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); + PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); + PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); + PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition); + PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); + + if (computeLinkVelocity) + { + pyLinkStateWorldLinkLinearVelocity = PyTuple_New(3); + pyLinkStateWorldLinkAngularVelocity = PyTuple_New(3); + for (i = 0; i < 3; ++i) + { + PyTuple_SetItem(pyLinkStateWorldLinkLinearVelocity, i, + PyFloat_FromDouble(linkState.m_worldLinearVelocity[i])); + PyTuple_SetItem(pyLinkStateWorldLinkAngularVelocity, i, + PyFloat_FromDouble(linkState.m_worldAngularVelocity[i])); + } + PyTuple_SetItem(pyLinkState, 6, pyLinkStateWorldLinkLinearVelocity); + PyTuple_SetItem(pyLinkState, 7, pyLinkStateWorldLinkAngularVelocity); + } + PyTuple_SetItem(resultListLinkState, link, pyLinkState); + + } + } + else + { + //invalid link, add a -1 result + PyTuple_SetItem(resultListLinkState, link, PyFloat_FromDouble(-1)); + } + } + Py_DECREF(linkIndicesSeq); + return resultListLinkState; + } + } + + Py_INCREF(Py_None); + return Py_None; +} + static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject* keywds) { b3SharedMemoryCommandHandle commandHandle; @@ -9772,6 +10668,8 @@ static PyObject* pybullet_executePluginCommand(PyObject* self, return PyInt_FromLong(statusType); } + + ///Inverse Kinematics binding static PyObject* pybullet_calculateInverseKinematics(PyObject* self, PyObject* args, PyObject* keywds) @@ -10675,6 +11573,8 @@ static PyObject* pybullet_calculateMassMatrix(PyObject* self, PyObject* args, Py return Py_None; } + + static PyMethodDef SpamMethods[] = { {"connect", (PyCFunction)pybullet_connectPhysicsServer, METH_VARARGS | METH_KEYWORDS, @@ -10890,12 +11790,16 @@ static PyMethodDef SpamMethods[] = { {"getJointState", (PyCFunction)pybullet_getJointState, METH_VARARGS | METH_KEYWORDS, "Get the state (position, velocity etc) for a joint on a body."}, - {"getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS, - "Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)"}, - {"getJointStates", (PyCFunction)pybullet_getJointStates, METH_VARARGS | METH_KEYWORDS, "Get the state (position, velocity etc) for multiple joints on a body."}, + { "getJointStateMultiDof", (PyCFunction)pybullet_getJointStateMultiDof, METH_VARARGS | METH_KEYWORDS, + "Get the state (position, velocity etc) for a joint on a body. (supports planar and spherical joints)" }, + + { "getJointStatesMultiDof", (PyCFunction)pybullet_getJointStatesMultiDof, METH_VARARGS | METH_KEYWORDS, + "Get the states (position, velocity etc) for multiple joint on a body. (supports planar and spherical joints)" }, + + {"getLinkState", (PyCFunction)pybullet_getLinkState, METH_VARARGS | METH_KEYWORDS, "position_linkcom_world, world_rotation_linkcom,\n" "position_linkcom_frame, frame_rotation_linkcom,\n" @@ -10907,6 +11811,9 @@ static PyMethodDef SpamMethods[] = { " center of mass (COM) of the link, relative to the world reference" " frame."}, + { "getLinkStates", (PyCFunction)pybullet_getLinkStates, METH_VARARGS | METH_KEYWORDS, + "same as getLinkState except it takes a list of linkIndices" }, + {"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS, "resetJointState(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n" "Reset the state (position, velocity etc) for a joint on a body " @@ -10916,6 +11823,10 @@ static PyMethodDef SpamMethods[] = { "resetJointStateMultiDof(objectUniqueId, jointIndex, targetValue, targetVelocity=0, physicsClientId=0)\n" "Reset the state (position, velocity etc) for a joint on a body " "instantaneously, not through physics simulation."}, + { "resetJointStatesMultiDof", (PyCFunction)pybullet_resetJointStatesMultiDof, METH_VARARGS | METH_KEYWORDS, + "resetJointStatesMultiDof(objectUniqueId, jointIndices, targetValues, targetVelocities=0, physicsClientId=0)\n" + "Reset the states (position, velocity etc) for multiple joints on a body " + "instantaneously, not through physics simulation." }, {"changeDynamics", (PyCFunction)pybullet_changeDynamicsInfo, METH_VARARGS | METH_KEYWORDS, "change dynamics information such as mass, lateral friction coefficient."}, @@ -10937,6 +11848,14 @@ static PyMethodDef SpamMethods[] = { "no immediate state change, stepSimulation will process the motors." "This method sets multi-degree-of-freedom motor such as the spherical joint motor."}, + { "setJointMotorControlMultiDofArray", (PyCFunction)pybullet_setJointMotorControlMultiDofArray, METH_VARARGS | METH_KEYWORDS, + "Set control mode and desired target values for multiple motors. There is " + "no immediate state change, stepSimulation will process the motors." + "This method sets multi-degree-of-freedom motor such as the spherical joint motor." }, + + + + {"setJointMotorControlArray", (PyCFunction)pybullet_setJointMotorControlArray, METH_VARARGS | METH_KEYWORDS, "Set an array of motors control mode and desired target value. There is " "no immediate state change, stepSimulation will process the motors." @@ -11119,6 +12038,7 @@ static PyMethodDef SpamMethods[] = { "Returns:\n" " massMatrix - a list of lists of the mass matrix components.\n"}, + {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, METH_VARARGS | METH_KEYWORDS, "Inverse Kinematics bindings: Given an object id, " @@ -11130,8 +12050,9 @@ static PyMethodDef SpamMethods[] = { "Inverse Kinematics bindings: Given an object id, " "current joint positions and target positions" " for the end effectors," - "compute the inverse kinematics and return the new joint state" }, - + "compute the inverse kinematics and return the new joint state" + }, + {"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, "Get Virtual Reality events, for example to track VR controllers position/buttons"}, {"setVRCameraState", (PyCFunction)pybullet_setVRCameraState, METH_VARARGS | METH_KEYWORDS, @@ -11316,7 +12237,8 @@ initpybullet(void) CONTROL_MODE_POSITION_VELOCITY_PD); // user read PyModule_AddIntConstant(m, "PD_CONTROL", CONTROL_MODE_PD); // user read - + PyModule_AddIntConstant(m, "STABLE_PD_CONTROL",CONTROL_MODE_STABLE_PD); + PyModule_AddIntConstant(m, "LINK_FRAME", EF_LINK_FRAME); PyModule_AddIntConstant(m, "WORLD_FRAME", EF_WORLD_FRAME);