Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Implement faster array versions of PyBullet: getJointStatesMultiDof, getLinkStates, setJointMotorControlMultiDofArray, resetJointStatesMultiDof,

Browse Source 
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?)
This commit is contained in:
Erwin Coumans
2019-07-21 13:08:22 -07:00
parent dff277ad7b
commit 39a4e8dcd9
12 changed files with 1503 additions and 171 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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;
}

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

241
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1788,32 +1788,11 @@ struct PhysicsServerCommandProcessorInternalData
#ifdef STATIC_LINK_SPD_PLUGIN
b3HashMap<btHashPtr, cRBDModel*> 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()); //??
@@ -1910,11 +1889,40 @@ struct PhysicsServerCommandProcessorInternalData
}
}
}
}
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);
{
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<double> zeroVel;
int dof = 7 + posVal;
zeroVel.resize(dof);
//clientCmd.m_sendDesiredStateCommandArgument.
//current positions and velocities
btAlignedObjectArray<double> jointPositionsQ;
btAlignedObjectArray<double> 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<Eigen::VectorXd> mKp((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kp, num_dof);
Eigen::Map<Eigen::VectorXd> mKd((double*)clientCmd.m_sendDesiredStateCommandArgument.m_Kd, num_dof);
Eigen::Map<Eigen::VectorXd> maxForce((double*)clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque, num_dof);
Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kp_mat = mKp.asDiagonal();
Eigen::DiagonalMatrix<double, Eigen::Dynamic> 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;
@@ -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;

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

17
examples/SharedMemory/plugins/stablePDPlugin/RBDModel.cpp
View File

@@ -1,7 +1,7 @@
#include "RBDModel.h"
#include "RBDUtil.h"
#include "KinTree.h"
#include "LinearMath/btQuickprof.h"
cRBDModel::cRBDModel()
{
}
@@ -38,12 +38,27 @@ void cRBDModel::Update(const Eigen::VectorXd& pose, const Eigen::VectorXd& vel)
SetPose(pose);
SetVel(vel);
{
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
{

10
examples/Utils/ChromeTraceUtil.cpp
View File

@@ -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;

62
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -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]])
@@ -412,7 +434,8 @@ while (p.isConnected()):
#for po in pose:
# print(po)
taus = stablePD.computePD(bodyUniqueId=humanoid,
taus = stablePD.computePD(bodyUniqueId=humanoid5,
jointIndices=jointIndicesAll,
desiredPositions=pose,
desiredVelocities=[0] * totalDofs,
@@ -421,6 +444,32 @@ while (p.isConnected()):
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,
desiredPositions=pose,
@@ -437,7 +486,7 @@ while (p.isConnected()):
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]])

127
examples/pybullet/gym/pybullet_envs/deep_mimic/env/humanoid_stable_pd.py vendored
View File

@@ -153,11 +153,26 @@ class HumanoidStablePD(object):
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)
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)
@@ -182,9 +197,20 @@ class HumanoidStablePD(object):
self._pybullet_client.resetJointStateMultiDof(phys_model, leftElbow, pose._leftElbowRot,
pose._leftElbowVel)
else:
if initBase:
self._pybullet_client.resetBasePositionAndOrientation(phys_model, pose._basePos,
pose._baseOrn)
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,
@@ -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,6 +369,29 @@ class HumanoidStablePD(object):
def applyPDForces(self, taus):
dofIndex = 7
scaling = 1
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:
@@ -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,15 +816,26 @@ 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]
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])
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])

8
examples/pybullet/gym/pybullet_envs/deep_mimic/env/pybullet_deep_mimic_env.py vendored
View File

@@ -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:
usePythonStablePD = False
if usePythonStablePD:
taus = self._humanoid.computePDForces(self.desiredPose,
desiredVelocities=None,
maxForces=maxForces)
#taus = [0]*43
self._humanoid.applyPDForces(taus)
else:
self._humanoid.computeAndApplyPDForces(self.desiredPose,
maxForces=maxForces)
else:
self._humanoid.setJointMotors(self.desiredPose, maxForces=maxForces)

14
examples/pybullet/gym/pybullet_envs/deep_mimic/env/testHumanoid.py vendored
View File

@@ -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)
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.

9
examples/pybullet/gym/pybullet_utils/pd_controller_stable.py
View File

@@ -62,7 +62,14 @@ 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):
if useArray:
js = jointStates[i]
else:
js = self._pb.getJointStateMultiDof(bodyUniqueId, jointIndices[i])
jointPos = js[0]
@@ -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

928
examples/pybullet/pybullet.c
View File

File diff suppressed because it is too large Load Diff