mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 22:20:12 +00:00
Merge pull request #1987 from erwincoumans/master
make setJointPosMultiDof and setJointVelMultiDof argument const.
This commit is contained in:
commit
38109dbd10
@ -38,7 +38,7 @@
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_baseLink_childA" type="continuous">
|
||||
<joint name="joint_baseLink_childA" type="spherical">
|
||||
<parent link="baseLink"/>
|
||||
<child link="childA"/>
|
||||
<origin xyz="0 0 0"/>
|
||||
@ -54,20 +54,21 @@
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
<box size="0.1 0.2 0.3 "/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
<box size="0.1 0.2 0.3 "/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_childA_childB" type="fixed">
|
||||
<joint name="joint_childA_childB" type="continuous">
|
||||
<parent link="childA"/>
|
||||
<child link="childB"/>
|
||||
<origin xyz="0 0 -0.2"/>
|
||||
<axis xyz="1 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
|
||||
|
@ -383,8 +383,65 @@ void ConvertURDF2BulletInternal(
|
||||
|
||||
switch (jointType)
|
||||
{
|
||||
case URDFFloatingJoint:
|
||||
case URDFSphericalJoint:
|
||||
{
|
||||
if (createMultiBody)
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
cache.m_bulletMultiBody->setupSpherical(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(), -offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
}
|
||||
else
|
||||
{
|
||||
btAssert(0);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFPlanarJoint:
|
||||
{
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
#if 0
|
||||
void setupPlanar(int i, // 0 to num_links-1
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
int parent,
|
||||
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
|
||||
const btVector3 &rotationAxis,
|
||||
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
|
||||
bool disableParentCollision = false);
|
||||
#endif
|
||||
creation.addLinkMapping(urdfLinkIndex, mbLinkIndex);
|
||||
cache.m_bulletMultiBody->setupPlanar(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(),
|
||||
disableParentCollision);
|
||||
}
|
||||
else
|
||||
{
|
||||
#if 0
|
||||
//b3Printf("Fixed joint\n");
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = 0;
|
||||
|
||||
//backward compatibility
|
||||
if (flags & CUF_RESERVED)
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
|
||||
}
|
||||
else
|
||||
{
|
||||
dof6 = creation.createFixedJoint(urdfLinkIndex, *linkRigidBody, *parentRigidBody, offsetInB, offsetInA);
|
||||
}
|
||||
if (enableConstraints)
|
||||
world1->addConstraint(dof6, true);
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
}
|
||||
case URDFFloatingJoint:
|
||||
|
||||
case URDFFixedJoint:
|
||||
{
|
||||
if ((jointType == URDFFloatingJoint) || (jointType == URDFPlanarJoint))
|
||||
@ -426,7 +483,9 @@ void ConvertURDF2BulletInternal(
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(), jointAxisInJointSpace), offsetInA.getOrigin(), //parent2joint.getOrigin(),
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),
|
||||
jointAxisInJointSpace),
|
||||
offsetInA.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
|
||||
|
@ -11,6 +11,8 @@ enum UrdfJointTypes
|
||||
URDFFloatingJoint,
|
||||
URDFPlanarJoint,
|
||||
URDFFixedJoint,
|
||||
URDFSphericalJoint,
|
||||
|
||||
};
|
||||
|
||||
enum URDF_LinkContactFlags
|
||||
|
@ -1280,7 +1280,9 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
|
||||
}
|
||||
|
||||
std::string type_str = type_char;
|
||||
if (type_str == "planar")
|
||||
if (type_str == "spherical")
|
||||
joint.m_type = URDFSphericalJoint;
|
||||
else if (type_str == "planar")
|
||||
joint.m_type = URDFPlanarJoint;
|
||||
else if (type_str == "floating")
|
||||
joint.m_type = URDFFloatingJoint;
|
||||
|
@ -972,6 +972,51 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState2* state)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||
b3Assert(status);
|
||||
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
|
||||
b3Assert(bodyIndex >= 0);
|
||||
if (bodyIndex >= 0)
|
||||
{
|
||||
state->m_qDofSize = 0;
|
||||
state->m_uDofSize = 0;
|
||||
b3JointInfo info;
|
||||
bool result = b3GetJointInfo(physClient, bodyIndex, jointIndex, &info) != 0;
|
||||
if (result)
|
||||
{
|
||||
|
||||
if ((info.m_qIndex >= 0) && (info.m_uIndex >= 0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
|
||||
{
|
||||
state->m_qDofSize = info.m_qSize;
|
||||
state->m_uDofSize = info.m_uSize;
|
||||
for (int i = 0; i < state->m_qDofSize; i++)
|
||||
{
|
||||
state->m_jointPosition[i] = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex + i];
|
||||
}
|
||||
for (int i = 0; i < state->m_uDofSize; i++)
|
||||
{
|
||||
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i];
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
state->m_jointPosition[0] = 0;
|
||||
state->m_jointVelocity[0] = 0;
|
||||
}
|
||||
for (int ii(0); ii < 6; ++ii)
|
||||
{
|
||||
state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
|
||||
}
|
||||
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState* state)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
|
||||
@ -1812,6 +1857,72 @@ B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle phys
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointPosition, int posSize)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_INIT_POSE);
|
||||
command->m_updateFlags |= INIT_POSE_HAS_JOINT_STATE;
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
|
||||
//if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >= 0)
|
||||
if (info.m_qIndex >= 0)
|
||||
{
|
||||
if (posSize == info.m_qSize)
|
||||
{
|
||||
for (int i = 0; i < posSize; i++)
|
||||
{
|
||||
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex + i] = jointPosition[i];
|
||||
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex + i] = 1;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_INIT_POSE);
|
||||
command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
|
||||
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
|
||||
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
|
||||
{
|
||||
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;
|
||||
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointVelocity, int velSize)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_INIT_POSE);
|
||||
command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
|
||||
|
||||
//if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
|
||||
if ((info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
|
||||
{
|
||||
if (velSize == info.m_uSize)
|
||||
{
|
||||
for (int i = 0; i < velSize; i++)
|
||||
{
|
||||
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex + i] = jointVelocity[i];
|
||||
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex + i] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
@ -1848,22 +1959,7 @@ B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle comman
|
||||
return 0;
|
||||
}
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity)
|
||||
{
|
||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
|
||||
b3Assert(command);
|
||||
b3Assert(command->m_type == CMD_INIT_POSE);
|
||||
command->m_updateFlags |= INIT_POSE_HAS_JOINT_VELOCITY;
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId, jointIndex, &info);
|
||||
//btAssert((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_uIndex >=0);
|
||||
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && (info.m_uIndex >= 0) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
|
||||
{
|
||||
command->m_initPoseArgs.m_initialStateQdot[info.m_uIndex] = jointVelocity;
|
||||
command->m_initPoseArgs.m_hasInitialStateQdot[info.m_uIndex] = 1;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId)
|
||||
{
|
||||
|
@ -524,13 +524,15 @@ extern "C"
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointPositionMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointPosition, int posSize);
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetQ(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* q, const int* hasQ);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetQdots(b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* qDots, const int* hasQdots);
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocities(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int numJointVelocities, const double* jointVelocities);
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocity(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointVelocity);
|
||||
|
||||
B3_SHARED_API int b3CreatePoseCommandSetJointVelocityMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, const double* jointVelocity, int velSize);
|
||||
|
||||
///We are currently not reading the sensor information from the URDF file, and programmatically assign sensors.
|
||||
///This is rather inconsistent, to mix programmatical creation with loading from file.
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId);
|
||||
@ -546,6 +548,7 @@ extern "C"
|
||||
B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics);
|
||||
|
||||
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState* state);
|
||||
B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState2* state);
|
||||
B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState* state);
|
||||
|
||||
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
|
||||
|
@ -149,6 +149,32 @@ bool PhysicsClientSharedMemory::getJointInfo(int bodyUniqueId, int jointIndex, b
|
||||
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
|
||||
{
|
||||
info = bodyJoints->m_jointInfo[jointIndex];
|
||||
switch (info.m_jointType)
|
||||
{
|
||||
case eSphericalType:
|
||||
{
|
||||
info.m_qSize = 4;//quaterion x,y,z,w
|
||||
info.m_uSize = 3;
|
||||
break;
|
||||
}
|
||||
case ePlanarType:
|
||||
{
|
||||
info.m_qSize = 2;
|
||||
info.m_uSize = 2;
|
||||
break;
|
||||
}
|
||||
case ePrismaticType:
|
||||
case eRevoluteType:
|
||||
{
|
||||
info.m_qSize = 1;
|
||||
info.m_uSize = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
@ -1291,6 +1291,37 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn
|
||||
if ((jointIndex >= 0) && (jointIndex < bodyJoints->m_jointInfo.size()))
|
||||
{
|
||||
info = bodyJoints->m_jointInfo[jointIndex];
|
||||
info.m_qSize = 0;
|
||||
info.m_uSize = 0;
|
||||
|
||||
switch (info.m_jointType)
|
||||
{
|
||||
case eSphericalType:
|
||||
{
|
||||
info.m_qSize = 4;//quaterion x,y,z,w
|
||||
info.m_uSize = 3;
|
||||
break;
|
||||
}
|
||||
case ePlanarType:
|
||||
{
|
||||
info.m_qSize = 2;
|
||||
info.m_uSize = 2;
|
||||
break;
|
||||
}
|
||||
case ePrismaticType:
|
||||
case eRevoluteType:
|
||||
{
|
||||
info.m_qSize = 1;
|
||||
info.m_uSize = 1;
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
@ -8007,17 +8007,39 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
|
||||
int posVarCountIndex = 7;
|
||||
for (int i = 0; i < mb->getNumLinks(); i++)
|
||||
{
|
||||
if ((clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex]) && (mb->getLink(i).m_dofCount == 1))
|
||||
bool hasPosVar = true;
|
||||
|
||||
for (int j = 0; j < mb->getLink(i).m_posVarCount; j++)
|
||||
{
|
||||
mb->setJointPos(i, clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
|
||||
mb->setJointVel(i, 0); //backwards compatibility
|
||||
if (clientCmd.m_initPoseArgs.m_hasInitialStateQ[posVarCountIndex+j] == 0)
|
||||
{
|
||||
hasPosVar = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if ((clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex]) && (mb->getLink(i).m_dofCount == 1))
|
||||
if (hasPosVar)
|
||||
{
|
||||
btScalar vel = clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex];
|
||||
mb->setJointVel(i, vel);
|
||||
mb->setJointPosMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQ[posVarCountIndex]);
|
||||
double vel[6] = { 0, 0, 0, 0, 0, 0 };
|
||||
mb->setJointVelMultiDof(i, vel);
|
||||
}
|
||||
|
||||
bool hasVel = true;
|
||||
for (int j = 0; j < mb->getLink(i).m_dofCount; j++)
|
||||
{
|
||||
if (clientCmd.m_initPoseArgs.m_hasInitialStateQdot[uDofIndex + j] == 0)
|
||||
{
|
||||
hasVel = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (hasVel)
|
||||
{
|
||||
mb->setJointVelMultiDof(i, &clientCmd.m_initPoseArgs.m_initialStateQdot[uDofIndex]);
|
||||
}
|
||||
|
||||
|
||||
posVarCountIndex += mb->getLink(i).m_posVarCount;
|
||||
uDofIndex += mb->getLink(i).m_dofCount;
|
||||
}
|
||||
|
@ -274,6 +274,8 @@ struct b3JointInfo
|
||||
double m_childFrame[7]; // ^^^
|
||||
double m_jointAxis[3]; // joint axis in parent local frame
|
||||
int m_parentIndex;
|
||||
int m_qSize;
|
||||
int m_uSize;
|
||||
};
|
||||
|
||||
enum UserDataValueType
|
||||
@ -357,6 +359,16 @@ struct b3JointSensorState
|
||||
double m_jointMotorTorque;
|
||||
};
|
||||
|
||||
struct b3JointSensorState2
|
||||
{
|
||||
double m_jointPosition[4];
|
||||
double m_jointVelocity[3];
|
||||
double m_jointReactionForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
|
||||
double m_jointMotorTorque;
|
||||
int m_qDofSize;
|
||||
int m_uDofSize;
|
||||
};
|
||||
|
||||
struct b3DebugLines
|
||||
{
|
||||
int m_numDebugLines;
|
||||
|
@ -9,8 +9,8 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1)
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(1)
|
||||
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
|
||||
print cid
|
||||
print p.getConstraintUniqueId(0)
|
||||
print (cid)
|
||||
print (p.getConstraintUniqueId(0))
|
||||
prev=[0,0,1]
|
||||
a=-math.pi
|
||||
while 1:
|
||||
|
116
examples/pybullet/examples/humanoidMotionCapture.py
Normal file
116
examples/pybullet/examples/humanoidMotionCapture.py
Normal file
@ -0,0 +1,116 @@
|
||||
import pybullet as p
|
||||
import json
|
||||
|
||||
p.connect(p.GUI)
|
||||
#p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
|
||||
|
||||
|
||||
import pybullet_data
|
||||
|
||||
useMotionCapture=True
|
||||
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
path = pybullet_data.getDataPath()+"/motions/humanoid3d_backflip.txt"
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
print("path = ", path)
|
||||
with open(path, 'r') as f:
|
||||
motion_dict = json.load(f)
|
||||
#print("motion_dict = ", motion_dict)
|
||||
print("len motion=", len(motion_dict))
|
||||
print(motion_dict['Loop'])
|
||||
numFrames = len(motion_dict['Frames'])
|
||||
print("#frames = ", numFrames)
|
||||
|
||||
frameId= p.addUserDebugParameter("frame",0,numFrames-1,0)
|
||||
|
||||
jointTypes = ["JOINT_REVOLUTE","JOINT_PRISMATIC",
|
||||
"JOINT_SPHERICAL","JOINT_PLANAR","JOINT_FIXED"]
|
||||
|
||||
humanoid = p.loadURDF("humanoid/humanoid.urdf", globalScaling=0.25)
|
||||
|
||||
for j in range (p.getNumJoints(humanoid)):
|
||||
ji = p.getJointInfo(humanoid,j)
|
||||
#print(ji)
|
||||
print("joint[",j,"].type=",jointTypes[ji[2]])
|
||||
print("joint[",j,"].name=",ji[1])
|
||||
|
||||
|
||||
jointIds=[]
|
||||
paramIds=[]
|
||||
for j in range (p.getNumJoints(humanoid)):
|
||||
p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
|
||||
p.changeVisualShape(humanoid,j,rgbaColor=[1,1,1,1])
|
||||
info = p.getJointInfo(humanoid,j)
|
||||
#print(info)
|
||||
if (not useMotionCapture):
|
||||
jointName = info[1]
|
||||
jointType = info[2]
|
||||
if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
|
||||
jointIds.append(j)
|
||||
paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
|
||||
print("jointName=",jointName, "at ", j)
|
||||
|
||||
p.changeVisualShape(humanoid,14,rgbaColor=[1,0,0,1])
|
||||
chest=1
|
||||
neck=2
|
||||
rightShoulder=3
|
||||
rightElbow=4
|
||||
leftShoulder=6
|
||||
leftElbow = 7
|
||||
rightHip = 9
|
||||
rightKnee=10
|
||||
rightAnkle=11
|
||||
leftHip = 12
|
||||
leftKnee=13
|
||||
leftAnkle=14
|
||||
|
||||
import time
|
||||
p.setRealTimeSimulation(0)
|
||||
while (1):
|
||||
p.getCameraImage(320,200)
|
||||
frame = int(p.readUserDebugParameter(frameId))
|
||||
frameData = motion_dict['Frames'][frame]
|
||||
#print("duration=",frameData[0])
|
||||
#print(pos=[frameData])
|
||||
basePos1 = [frameData[1],frameData[2],frameData[3]]
|
||||
baseOrn1 = [frameData[5],frameData[6], frameData[7],frameData[4]]
|
||||
#pre-rotate to make z-up
|
||||
y2zPos=[0,0,0]
|
||||
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
|
||||
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
|
||||
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
|
||||
chestRot = [frameData[9],frameData[10],frameData[11],frameData[8]]
|
||||
neckRot = [frameData[13],frameData[14],frameData[15],frameData[12]]
|
||||
rightHipRot = [frameData[17],frameData[18],frameData[19],frameData[16]]
|
||||
rightKneeRot = [frameData[20]]
|
||||
rightAnkleRot = [frameData[22],frameData[23],frameData[24],frameData[21]]
|
||||
rightShoulderRot = [frameData[26],frameData[27],frameData[28],frameData[25]]
|
||||
rightElbowRot = [frameData[29]]
|
||||
leftHipRot = [frameData[31],frameData[32],frameData[33],frameData[30]]
|
||||
leftKneeRot = [frameData[34]]
|
||||
leftAnkleRot = [frameData[36],frameData[37],frameData[38],frameData[35]]
|
||||
leftShoulderRot = [frameData[40],frameData[41],frameData[42],frameData[39]]
|
||||
leftElbowRot = [frameData[43]]
|
||||
#print("chestRot=",chestRot)
|
||||
if (useMotionCapture):
|
||||
p.resetJointStateMultiDof(humanoid,chest,chestRot)
|
||||
p.resetJointStateMultiDof(humanoid,neck,neckRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightHip,rightHipRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightKnee,rightKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightAnkle,rightAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightShoulder,rightShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid,rightElbow, rightElbowRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftHip, leftHipRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftKnee, leftKneeRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftAnkle, leftAnkleRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftShoulder, leftShoulderRot)
|
||||
p.resetJointStateMultiDof(humanoid,leftElbow, leftElbowRot)
|
||||
else:
|
||||
for i in range(len(paramIds)):
|
||||
c = paramIds[i]
|
||||
targetPos = p.readUserDebugParameter(c)
|
||||
|
||||
p.setJointMotorControl2(humanoid,jointIds[i],
|
||||
p.POSITION_CONTROL,targetPos, force=5*240.)
|
||||
time.sleep(0.1)
|
284
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
Normal file
284
examples/pybullet/gym/pybullet_data/humanoid/humanoid.urdf
Normal file
@ -0,0 +1,284 @@
|
||||
<robot name="dumpUrdf">
|
||||
<link name="base" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0 0 0" />
|
||||
<mass value = "0.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="link0" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.280000 0.000000" />
|
||||
<mass value = "6.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.280000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint0" type="fixed" >
|
||||
<parent link = "base" />
|
||||
<child link="link0" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
</joint>
|
||||
<link name="link1" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.480000 0.000000" />
|
||||
<mass value = "14.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.480000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.440000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint1" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link1" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.944604 0.000000" />
|
||||
</joint>
|
||||
<link name="link2" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.700000 0.000000" />
|
||||
<mass value = "2.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.700000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.410000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint2" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link2" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.895576 0.000000" />
|
||||
</joint>
|
||||
<link name="link3" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<mass value = "4.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.200000" radius="0.220000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint3" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link3" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.339548" />
|
||||
</joint>
|
||||
<link name="link4" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<mass value = "3.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.240000" radius="0.200000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint4" type="continuous" >
|
||||
<parent link="link3" />
|
||||
<child link="link4" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link5" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<geometry>
|
||||
<box size="0.708000 0.220000 0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint5" type="spherical" >
|
||||
<parent link="link4" />
|
||||
<child link="link5" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
|
||||
</joint>
|
||||
<link name="link6" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<mass value = "1.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.720000" radius="0.180000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint6" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link6" />
|
||||
<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 0.732440" />
|
||||
</joint>
|
||||
<link name="link7" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.540000" radius="0.160000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint7" type="continuous" >
|
||||
<parent link="link6" />
|
||||
<child link="link7" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link8" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<mass value = "0.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.160000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint8" type="fixed" >
|
||||
<parent link="link7" />
|
||||
<child link="link8" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
|
||||
</joint>
|
||||
<link name="link9" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<mass value = "4.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.840000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.200000" radius="0.220000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint9" type="spherical" >
|
||||
<parent link="link0" />
|
||||
<child link="link9" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 -0.339548" />
|
||||
</joint>
|
||||
<link name="link10" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<mass value = "3.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.800000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="1.240000" radius="0.200000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint10" type="continuous" >
|
||||
<parent link="link9" />
|
||||
<child link="link10" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.686184 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link11" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.180000 -0.090000 0.000000" />
|
||||
<geometry>
|
||||
<box size="0.708000 0.220000 0.360000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint11" type="spherical" >
|
||||
<parent link="link10" />
|
||||
<child link="link11" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.639480 0.000000" />
|
||||
</joint>
|
||||
<link name="link12" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<mass value = "1.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.560000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.720000" radius="0.180000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint12" type="spherical" >
|
||||
<parent link="link1" />
|
||||
<child link="link12" />
|
||||
<origin rpy = "0 0 0" xyz = "-0.096200 0.974000 -0.732440" />
|
||||
</joint>
|
||||
<link name="link13" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<mass value = "1.000000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "-1.570796 0 0" xyz = "0.000000 -0.480000 0.000000" />
|
||||
<geometry>
|
||||
<capsule length="0.540000" radius="0.160000"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint13" type="continuous" >
|
||||
<parent link="link12" />
|
||||
<child link="link13" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.099152 0.000000" />
|
||||
<axis xyz = "0.000000 0.000000 1.000000" />
|
||||
</joint>
|
||||
<link name="link14" >
|
||||
<inertial>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<mass value = "0.500000" />
|
||||
<inertia ixx = "0.1" ixy = "0" ixz = "0" iyy = "0.1" iyz = "0" izz = "0.1" />
|
||||
</inertial>
|
||||
<collision>
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 0.000000 0.000000" />
|
||||
<geometry>
|
||||
<sphere radius = "0.160000" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint14" type="fixed" >
|
||||
<parent link="link13" />
|
||||
<child link="link14" />
|
||||
<origin rpy = "0 0 0" xyz = "0.000000 -1.035788 0.000000" />
|
||||
</joint>
|
||||
</robot>
|
||||
|