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

tweaks in pybullet and shared memory C-API:

Browse Source 
allow to reset the state of a single joint
allow to set the target/mode for a single joint motor at a time
rename pybullet API: initializeJointPositions -> resetJointState
This commit is contained in:
Erwin Coumans (Google)
2016-06-24 07:31:17 -07:00
parent 2cd0eba257
commit 6d1948e79e
7 changed files with 463 additions and 413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/BasicDemo/BasicExample.cpp
View File

@@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
float dist = 41; float dist = 41;
float pitch = 52; float pitch = 52;
float yaw = 35; float yaw = 35;
float targetPos[3]={0,0.46,0}; float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
} }
}; };

33
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -167,6 +167,10 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle ph
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId; command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0; command->m_updateFlags = 0;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
}
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -176,6 +180,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
return 0; return 0;
} }
@@ -186,6 +191,8 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
return 0; return 0;
} }
@@ -195,6 +202,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
return 0; return 0;
} }
@@ -205,6 +213,8 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
return 0; return 0;
} }
@@ -215,6 +225,8 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0; return 0;
} }
@@ -224,6 +236,8 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0; return 0;
} }
@@ -372,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl); b3Assert(cl);
b3Assert(cl->canSubmitCommand()); b3Assert(cl->canSubmitCommand());
@@ -380,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
command->m_type = CMD_INIT_POSE; command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0; command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex; command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
}
return (b3SharedMemoryCommandHandle) command; return (b3SharedMemoryCommandHandle) command;
} }
@@ -392,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
command->m_initPoseArgs.m_initialStateQ[0] = startPosX; command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY; command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ; command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
return 0; return 0;
} }
@@ -405,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY; command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ; command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW; command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
return 0; return 0;
} }
@@ -417,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
for (int i=0;i<numJointPositions;i++) for (int i=0;i<numJointPositions;i++)
{ {
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
} }
return 0; return 0;
} }
@@ -433,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{ {
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
} }
return 0; return 0;
} }

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -99,15 +99,16 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint control variables such as desired position/angle, desired velocity, ///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE, ///Only use if when controlMode is CONTROL_MODE_TORQUE,

145
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1386,38 +1386,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
b3Printf("Using CONTROL_MODE_TORQUE"); b3Printf("Using CONTROL_MODE_TORQUE");
} }
mb->clearForcesAndTorques(); // mb->clearForcesAndTorques();
int torqueIndex = 0; int torqueIndex = 0;
#if 0 if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
//it is inconsistent to allow application of base force/torque, there is no 'joint' motor. Use a separate API for this.
btVector3 f(0,0,0);
btVector3 t(0,0,0);
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
}
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
#endif
for (int link=0;link<mb->getNumLinks();link++)
{ {
for (int link=0;link<mb->getNumLinks();link++)
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{ {
double torque = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; {
mb->addJointTorqueMultiDof(link,dof,torque); double torque = 0.f;
torqueIndex++; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
}
torqueIndex++;
}
} }
} }
break; break;
@@ -1444,15 +1429,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (motor) if (motor)
{ {
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) bool hasDesiredVelocity = false;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0)
{
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity); motor->setVelocityTarget(desiredVelocity);
hasDesiredVelocity = true;
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; }
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) if (hasDesiredVelocity)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; {
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp); if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
}
motor->setMaxAppliedImpulse(maxImp);
}
numMotors++; numMotors++;
} }
@@ -1470,7 +1463,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD");
} }
//compute the force base on PD control //compute the force base on PD control
mb->clearForcesAndTorques();
int numMotors = 0; int numMotors = 0;
//find the joint motors and apply the desired velocity and maximum force/torque //find the joint motors and apply the desired velocity and maximum force/torque
@@ -1488,37 +1480,54 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (motor) if (motor)
{ {
btScalar desiredVelocity = 0.f; bool hasDesiredPosOrVel = false;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0)
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar kp = 0.f; btScalar kp = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0) btScalar kd = 0.f;
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; btScalar desiredVelocity = 0.f;
btScalar kd = 0.f; if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0)
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0) {
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; hasDesiredPosOrVel = true;
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
kd = 0.1;
}
btScalar desiredPosition = 0.f;
if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0)
{
hasDesiredPosOrVel = true;
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
kp = 0.1;
}
int dof1 = 0; if (hasDesiredPosOrVel)
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; {
btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1];
btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime;
btScalar velocityError = (desiredVelocity - currentVelocity);
desiredVelocity = kp * positionStabiliationTerm + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0)
kd * velocityError; {
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
}
motor->setVelocityTarget(desiredVelocity); if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0)
{
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
}
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; int dof1 = 0;
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1];
btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime;
btScalar velocityError = (desiredVelocity - currentVelocity);
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) desiredVelocity = kp * positionStabiliationTerm +
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; kd * velocityError;
motor->setMaxAppliedImpulse(maxImp); motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
}
numMotors++; numMotors++;
} }
@@ -1803,6 +1812,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
{ {
btVector3 zero(0,0,0); btVector3 zero(0,0,0);
btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]);
mb->setBaseVel(zero); mb->setBaseVel(zero);
mb->setBasePos(btVector3( mb->setBasePos(btVector3(
clientCmd.m_initPoseArgs.m_initialStateQ[0], clientCmd.m_initPoseArgs.m_initialStateQ[0],
@@ -1811,6 +1824,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
{ {
btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] &&
clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]);
mb->setBaseOmega(btVector3(0,0,0)); mb->setBaseOmega(btVector3(0,0,0));
mb->setWorldToBaseRot(btQuaternion( mb->setWorldToBaseRot(btQuaternion(
clientCmd.m_initPoseArgs.m_initialStateQ[3], clientCmd.m_initPoseArgs.m_initialStateQ[3],
@@ -1823,9 +1841,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int dofIndex = 7; int dofIndex = 7;
for (int i=0;i<mb->getNumLinks();i++) for (int i=0;i<mb->getNumLinks();i++)
{ {
if (mb->getLink(i).m_dofCount==1) if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1))
{ {
mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
mb->setJointVel(i,0); mb->setJointVel(i,0);
} }

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -115,6 +115,7 @@ enum EnumInitPoseFlags
struct InitPoseArgs struct InitPoseArgs
{ {
int m_bodyUniqueId; int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
}; };
@@ -179,12 +180,15 @@ struct SendDesiredStateArgs
double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
//desired state is only written by the client, read-only access by server is expected //desired state is only written by the client, read-only access by server is expected
//m_desiredStateQ is indexed by position variables, //m_desiredStateQ is indexed by position variables,
//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables //starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables //m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];

225
examples/pybullet/pybullet.c
View File

@@ -3,6 +3,7 @@
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#ifdef __APPLE__ #ifdef __APPLE__
#include <Python/Python.h> #include <Python/Python.h>
#else #else
@@ -139,9 +140,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
int bodyIndex = -1; int bodyIndex = -1;
const char* urdfFileName=""; const char* urdfFileName="";
float startPosX =0; float startPosX =0;
float startPosY =0; float startPosY =0;
float startPosZ = 1; float startPosZ = 0;
float startOrnX = 0; float startOrnX = 0;
float startOrnY = 0; float startOrnY = 0;
float startOrnZ = 0; float startOrnZ = 0;
@@ -282,7 +285,7 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
return Py_None; return Py_None;
} }
static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
//todo(erwincoumans): set max forces, kp, kd //todo(erwincoumans): set max forces, kp, kd
@@ -296,25 +299,22 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
size= PySequence_Size(args); size= PySequence_Size(args);
if (size==3) if (size==4)
{ {
int bodyIndex, controlMode; int bodyIndex, jointIndex, controlMode;
PyObject* targetValues; double targetValue;
if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues))
if (PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{ {
PyObject* seq; int numJoints;
int numJoints, len;
seq = PySequence_Fast(targetValues, "expected a sequence");
len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int qIndex; struct b3JointInfo info;
if (len!=numJoints) numJoints = b3GetNumJoints(sm,bodyIndex);
{ if ((jointIndex >= numJoints) || (jointIndex < 0))
PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); {
Py_DECREF(seq); PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL; return NULL;
} }
@@ -323,49 +323,43 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{ {
PyErr_SetString(SpamError, "Illegral control mode."); PyErr_SetString(SpamError, "Illegral control mode.");
Py_DECREF(seq);
return NULL; return NULL;
} }
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
for (qIndex=0;qIndex<numJoints;qIndex++) b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{ {
float value = pybullet_internalGetFloatFromSequence(seq,qIndex); case CONTROL_MODE_VELOCITY:
switch (controlMode)
{ {
case CONTROL_MODE_VELOCITY: b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
{ b3JointControlSetKd(commandHandle,info.m_uIndex,1);
b3JointControlSetDesiredVelocity(commandHandle, 6+qIndex, value); b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000);
b3JointControlSetKd(commandHandle,6+qIndex,1); break;
b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000); }
break;
}
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
b3JointControlSetDesiredForceTorque(commandHandle, 6+qIndex, value); b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{ {
b3JointControlSetDesiredPosition( commandHandle, 7+qIndex, value); b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
b3JointControlSetKp(commandHandle,6+qIndex,1); b3JointControlSetKp(commandHandle,info.m_uIndex,1);
b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000); b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,10000);
break; break;
} }
default: default:
{ {
}
};
} statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
};
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_DECREF(seq);
Py_INCREF(Py_None); Py_INCREF(Py_None);
return Py_None; return Py_None;
} }
@@ -560,7 +554,7 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
// Initalize all joint positions given a list of values // Initalize all joint positions given a list of values
static PyObject* static PyObject*
pybullet_initializeJointPositions(PyObject* self, PyObject* args) pybullet_resetJointState(PyObject* self, PyObject* args)
{ {
int size; int size;
if (0==sm) if (0==sm)
@@ -572,44 +566,36 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args)
size= PySequence_Size(args); size= PySequence_Size(args);
if (size==2) if (size==3)
{ {
int bodyIndex; int bodyIndex;
PyObject* targetValues; int jointIndex;
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetValues)) double targetValue;
if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue))
{ {
PyObject* seq;
int numJoints, len;
seq = PySequence_Fast(targetValues, "expected a sequence");
len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int qIndex; int numJoints;
if (len!=numJoints) numJoints = b3GetNumJoints(sm,bodyIndex);
{ if ((jointIndex >= numJoints) || (jointIndex < 0))
PyErr_SetString(SpamError, "Number of joint position values doesn't match the number of joints."); {
Py_DECREF(seq); PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL; return NULL;
} }
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
for (qIndex=0;qIndex<numJoints;qIndex++) b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue);
{
float value = pybullet_internalGetFloatFromSequence(seq,qIndex);
b3CreatePoseCommandSetJointPosition(sm, commandHandle, qIndex, value);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_DECREF(seq); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None); Py_INCREF(Py_None);
return Py_None; return Py_None;
} }
} }
PyErr_SetString(SpamError, "error in initializeJointPositions."); PyErr_SetString(SpamError, "error in resetJointState.");
return NULL; return NULL;
} }
@@ -658,7 +644,7 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args)
// Py_INCREF(Py_None); // Py_INCREF(Py_None);
// return Py_None; // return Py_None;
// } // }
#if 0
static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) { static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) {
int i, j; int i, j;
int numDegreeQ; int numDegreeQ;
@@ -746,7 +732,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args)
Py_INCREF(Py_None); Py_INCREF(Py_None);
return Py_None; return Py_None;
} }
#endif
// Get the a single joint info for a specific bodyIndex // Get the a single joint info for a specific bodyIndex
// //
@@ -765,13 +751,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args)
static PyObject* static PyObject*
pybullet_getJointInfo(PyObject* self, PyObject* args) pybullet_getJointInfo(PyObject* self, PyObject* args)
{ {
if (0==sm) PyObject *pyListJointInfo;
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
PyObject *pyListJointInfo;
struct b3JointInfo info; struct b3JointInfo info;
@@ -781,6 +761,14 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
int size= PySequence_Size(args); int size= PySequence_Size(args);
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==2) // get body index and joint index if (size==2) // get body index and joint index
{ {
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
@@ -808,7 +796,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
// info.m_jointDamping, // info.m_jointDamping,
// info.m_jointFriction); // info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0, PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex)); PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1, PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName)); PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2, PyTuple_SetItem(pyListJointInfo, 2,
@@ -848,14 +836,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
static PyObject* static PyObject*
pybullet_getJointState(PyObject* self, PyObject* args) pybullet_getJointState(PyObject* self, PyObject* args)
{ {
if (0==sm) PyObject *pyListJointForceTorque;
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
PyObject *pyListJointForceTorque;
PyObject *pyListJointState; PyObject *pyListJointState;
PyObject *item; PyObject *item;
@@ -871,6 +852,15 @@ pybullet_getJointState(PyObject* self, PyObject* args)
int size= PySequence_Size(args); int size= PySequence_Size(args);
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==2) // get body index and joint index if (size==2) // get body index and joint index
{ {
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex)) if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
@@ -1070,11 +1060,6 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
static PyMethodDef SpamMethods[] = { static PyMethodDef SpamMethods[] = {
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"connect", pybullet_connectPhysicsServer, METH_VARARGS, {"connect", pybullet_connectPhysicsServer, METH_VARARGS,
"Connect to an existing physics server (using shared memory by default)."}, "Connect to an existing physics server (using shared memory by default)."},
@@ -1091,35 +1076,45 @@ static PyMethodDef SpamMethods[] = {
{"setGravity", pybullet_setGravity, METH_VARARGS, {"setGravity", pybullet_setGravity, METH_VARARGS,
"Set the gravity acceleration (x,y,z)."}, "Set the gravity acceleration (x,y,z)."},
{"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS, {"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."}, "Create a multibody by loading a URDF file."},
// CURRENTLY NOT SUPPORTED {"loadSDF", pybullet_loadSDF, METH_VARARGS,
// {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS, "Load multibodies from an SDF file."},
// "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."},
{"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
"Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."},
{"getJointState", pybullet_getJointState, METH_VARARGS,
"Get the joint metadata info for a joint on a body."},
{"setJointControl", pybullet_setJointControl, METH_VARARGS,
"Set the joint control mode and desired target values."},
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS, {"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
"Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."}, "Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getJointPositions", pybullet_getJointPositions, METH_VARARGS, // {"resetBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
"Get the all the joint positions for a given body index."}, // "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS, {"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."}, "Get the number of joints for an object."},
{"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
"Get the name and type info for a joint on a body."},
{"getJointState", pybullet_getJointState, METH_VARARGS,
"Get the state (position, velocity etc) for a joint on a body."},
{"resetJointState", pybullet_resetJointState, METH_VARARGS,
"Reset the state (position, velocity etc) for a joint on a body instantaneously, not through physics simulation."},
{"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
"Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."},
//saveSnapshot
//loadSnapshot
//collision info
//raycast info
//applyBaseForce
//applyLinkForce
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{NULL, NULL, 0, NULL} /* Sentinel */ {NULL, NULL, 0, NULL} /* Sentinel */
}; };

4
test/SharedMemory/test.c
View File

@@ -85,9 +85,9 @@ void testSharedMemory(b3PhysicsClientHandle sm)
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
double jointAngle = 0.f; double jointAngle = 0.f;
int jointIndex;
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
for (int jointIndex=0;jointIndex<numJoints;jointIndex++) for (jointIndex=0;jointIndex<numJoints;jointIndex++)
{ {
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle); b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
} }