mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
add quadruped.py script to load and initialize the a Minitaur-like quadruped
pybullet removeConstraint, createConstraint rename b3CreateJoint to b3InitCreateUserConstraintCommand add int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
This commit is contained in:
erwin coumans
parent
c521d816c6
commit
c0fb98861d
@ -960,7 +960,9 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
|
||||
return cl->getJointInfo(bodyIndex, jointIndex, *info);
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
@ -968,22 +970,52 @@ b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
|
||||
command->m_type = CMD_CREATE_JOINT;
|
||||
command->m_createJointArguments.m_parentBodyIndex = parentBodyIndex;
|
||||
command->m_createJointArguments.m_parentJointIndex = parentJointIndex;
|
||||
command->m_createJointArguments.m_childBodyIndex = childBodyIndex;
|
||||
command->m_createJointArguments.m_childJointIndex = childJointIndex;
|
||||
command->m_type = CMD_USER_CONSTRAINT;
|
||||
command->m_updateFlags = USER_CONSTRAINT_ADD_CONSTRAINT;
|
||||
|
||||
command->m_userConstraintArguments.m_parentBodyIndex = parentBodyIndex;
|
||||
command->m_userConstraintArguments.m_parentJointIndex = parentJointIndex;
|
||||
command->m_userConstraintArguments.m_childBodyIndex = childBodyIndex;
|
||||
command->m_userConstraintArguments.m_childJointIndex = childJointIndex;
|
||||
for (int i = 0; i < 7; ++i) {
|
||||
command->m_createJointArguments.m_parentFrame[i] = info->m_parentFrame[i];
|
||||
command->m_createJointArguments.m_childFrame[i] = info->m_childFrame[i];
|
||||
command->m_userConstraintArguments.m_parentFrame[i] = info->m_parentFrame[i];
|
||||
command->m_userConstraintArguments.m_childFrame[i] = info->m_childFrame[i];
|
||||
}
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
command->m_createJointArguments.m_jointAxis[i] = info->m_jointAxis[i];
|
||||
command->m_userConstraintArguments.m_jointAxis[i] = info->m_jointAxis[i];
|
||||
}
|
||||
command->m_createJointArguments.m_jointType = info->m_jointType;
|
||||
command->m_userConstraintArguments.m_jointType = info->m_jointType;
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
|
||||
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
|
||||
{
|
||||
PhysicsClient* cl = (PhysicsClient* ) physClient;
|
||||
b3Assert(cl);
|
||||
b3Assert(cl->canSubmitCommand());
|
||||
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
|
||||
b3Assert(command);
|
||||
|
||||
command->m_type = CMD_USER_CONSTRAINT;
|
||||
command->m_updateFlags = USER_CONSTRAINT_REMOVE_CONSTRAINT;
|
||||
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
|
||||
return (b3SharedMemoryCommandHandle)command;
|
||||
}
|
||||
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
b3Assert(status);
|
||||
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
|
||||
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
|
||||
{
|
||||
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
|
||||
}
|
||||
|
||||
return -1;
|
||||
|
||||
}
|
||||
|
||||
|
||||
b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
|
||||
double rayFromWorldY, double rayFromWorldZ,
|
||||
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
|
||||
|
||||
@ -72,7 +72,9 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
|
||||
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
|
||||
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
|
||||
|
||||
b3SharedMemoryCommandHandle b3CreateJoint(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
|
||||
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
|
||||
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
|
||||
|
||||
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
|
||||
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h
|
||||
|
||||
@ -731,7 +731,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Warning("User debug draw failed");
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_USER_CONSTRAINT_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_USER_CONSTRAINT_FAILED:
|
||||
{
|
||||
b3Warning("createConstraint failed");
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
b3Error("Unknown server status %d\n", serverCmd.m_type);
|
||||
btAssert(0);
|
||||
|
||||
@ -671,10 +671,18 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
|
||||
{
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_USER_CONSTRAINT_COMPLETED:
|
||||
{
|
||||
break;
|
||||
}
|
||||
case CMD_USER_CONSTRAINT_FAILED:
|
||||
{
|
||||
b3Warning("createConstraint failed");
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
b3Warning("Unknown server status type");
|
||||
//b3Warning("Unknown server status type");
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -123,6 +123,17 @@ struct InteralBodyData
|
||||
}
|
||||
};
|
||||
|
||||
struct InteralUserConstraintData
|
||||
{
|
||||
btTypedConstraint* m_rbConstraint;
|
||||
btMultiBodyConstraint* m_mbConstraint;
|
||||
InteralUserConstraintData()
|
||||
:m_rbConstraint(0),
|
||||
m_mbConstraint(0)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
///todo: templatize this
|
||||
struct InternalBodyHandle : public InteralBodyData
|
||||
{
|
||||
@ -468,6 +479,10 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_multiBodyJointFeedbacks;
|
||||
btHashMap<btHashPtr, btInverseDynamics::MultiBodyTree*> m_inverseDynamicsBodies;
|
||||
btHashMap<btHashPtr, IKTrajectoryHelper*> m_inverseKinematicsHelpers;
|
||||
|
||||
int m_userConstraintUIDGenerator;
|
||||
btHashMap<btHashInt, InteralUserConstraintData> m_userConstraints;
|
||||
|
||||
b3AlignedObjectArray<SaveWorldObjectData> m_saveWorldBodyData;
|
||||
|
||||
|
||||
@ -532,6 +547,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
m_logPlayback(0),
|
||||
m_physicsDeltaTime(1./240.),
|
||||
m_numSimulationSubSteps(0),
|
||||
m_userConstraintUIDGenerator(1),
|
||||
m_dynamicsWorld(0),
|
||||
m_remoteDebugDrawer(0),
|
||||
m_guiHelper(0),
|
||||
@ -3097,72 +3113,144 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_CREATE_JOINT:
|
||||
case CMD_USER_CONSTRAINT:
|
||||
{
|
||||
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_createJointArguments.m_parentBodyIndex);
|
||||
if (parentBody && parentBody->m_multiBody)
|
||||
{
|
||||
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_createJointArguments.m_childBodyIndex);
|
||||
if (childBody)
|
||||
{
|
||||
btVector3 pivotInParent(clientCmd.m_createJointArguments.m_parentFrame[0], clientCmd.m_createJointArguments.m_parentFrame[1], clientCmd.m_createJointArguments.m_parentFrame[2]);
|
||||
btVector3 pivotInChild(clientCmd.m_createJointArguments.m_childFrame[0], clientCmd.m_createJointArguments.m_childFrame[1], clientCmd.m_createJointArguments.m_childFrame[2]);
|
||||
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_createJointArguments.m_parentFrame[3], clientCmd.m_createJointArguments.m_parentFrame[4], clientCmd.m_createJointArguments.m_parentFrame[5], clientCmd.m_createJointArguments.m_parentFrame[6]));
|
||||
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_createJointArguments.m_childFrame[3], clientCmd.m_createJointArguments.m_childFrame[4], clientCmd.m_createJointArguments.m_childFrame[5], clientCmd.m_createJointArguments.m_childFrame[6]));
|
||||
btVector3 jointAxis(clientCmd.m_createJointArguments.m_jointAxis[0], clientCmd.m_createJointArguments.m_jointAxis[1], clientCmd.m_createJointArguments.m_jointAxis[2]);
|
||||
if (clientCmd.m_createJointArguments.m_jointType == eFixedType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
multibodyFixed->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
rigidbodyFixed->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodyFixed);
|
||||
}
|
||||
}
|
||||
else if (clientCmd.m_createJointArguments.m_jointType == ePrismaticType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
multibodySlider->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
rigidbodySlider->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodySlider);
|
||||
}
|
||||
} else if (clientCmd.m_createJointArguments.m_jointType == ePoint2PointType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_createJointArguments.m_childJointIndex,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_createJointArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
SharedMemoryStatus& serverCmd =serverStatusOut;
|
||||
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
|
||||
hasStatus = true;
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_CONSTRAINT_ADD_CONSTRAINT)
|
||||
{
|
||||
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
|
||||
if (parentBody && parentBody->m_multiBody)
|
||||
{
|
||||
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex);
|
||||
if (childBody)
|
||||
{
|
||||
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
|
||||
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
|
||||
btMatrix3x3 frameInParent(btQuaternion(clientCmd.m_userConstraintArguments.m_parentFrame[3], clientCmd.m_userConstraintArguments.m_parentFrame[4], clientCmd.m_userConstraintArguments.m_parentFrame[5], clientCmd.m_userConstraintArguments.m_parentFrame[6]));
|
||||
btMatrix3x3 frameInChild(btQuaternion(clientCmd.m_userConstraintArguments.m_childFrame[3], clientCmd.m_userConstraintArguments.m_childFrame[4], clientCmd.m_userConstraintArguments.m_childFrame[5], clientCmd.m_userConstraintArguments.m_childFrame[6]));
|
||||
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
|
||||
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
multibodyFixed->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodyFixed);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = multibodyFixed;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
|
||||
rigidbodyFixed->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodyFixed);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = rigidbodyFixed;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
}
|
||||
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
multibodySlider->setMaxAppliedImpulse(500.0);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(multibodySlider);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = multibodySlider;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
|
||||
rigidbodySlider->setMaxAppliedImpulse(500.0);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(rigidbodySlider);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = rigidbodySlider;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
|
||||
{
|
||||
if (childBody->m_multiBody)
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
m_data->m_dynamicsWorld->addMultiBodyConstraint(p2p);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = p2p;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
else
|
||||
{
|
||||
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
|
||||
p2p->setMaxAppliedImpulse(500);
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
world->addMultiBodyConstraint(p2p);
|
||||
InteralUserConstraintData userConstraintData;
|
||||
userConstraintData.m_mbConstraint = p2p;
|
||||
int uid = m_data->m_userConstraintUIDGenerator++;
|
||||
m_data->m_userConstraints.insert(uid,userConstraintData);
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
b3Warning("unknown constraint type");
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
|
||||
{
|
||||
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
|
||||
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
|
||||
if (userConstraintPtr)
|
||||
{
|
||||
if (userConstraintPtr->m_mbConstraint)
|
||||
{
|
||||
m_data->m_dynamicsWorld->removeMultiBodyConstraint(userConstraintPtr->m_mbConstraint);
|
||||
delete userConstraintPtr->m_mbConstraint;
|
||||
m_data->m_userConstraints.remove(userConstraintUidRemove);
|
||||
}
|
||||
if (userConstraintPtr->m_rbConstraint)
|
||||
{
|
||||
|
||||
}
|
||||
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
|
||||
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
case CMD_CALCULATE_INVERSE_KINEMATICS:
|
||||
|
||||
@ -488,7 +488,14 @@ struct CalculateInverseKinematicsResultArgs
|
||||
double m_jointPositions[MAX_DEGREE_OF_FREEDOM];
|
||||
};
|
||||
|
||||
struct CreateJointArgs
|
||||
enum EnumUserConstraintFlags
|
||||
{
|
||||
USER_CONSTRAINT_ADD_CONSTRAINT=1,
|
||||
USER_CONSTRAINT_REMOVE_CONSTRAINT=2,
|
||||
USER_CONSTRAINT_CHANGE_CONSTRAINT=4
|
||||
};
|
||||
|
||||
struct UserConstraintArgs
|
||||
{
|
||||
int m_parentBodyIndex;
|
||||
int m_parentJointIndex;
|
||||
@ -498,9 +505,13 @@ struct CreateJointArgs
|
||||
double m_childFrame[7];
|
||||
double m_jointAxis[3];
|
||||
int m_jointType;
|
||||
int m_userConstraintUniqueId;
|
||||
};
|
||||
|
||||
|
||||
struct UserConstraintResultArgs
|
||||
{
|
||||
int m_userConstraintUniqueId;
|
||||
};
|
||||
|
||||
enum EnumUserDebugDrawFlags
|
||||
{
|
||||
@ -563,7 +574,7 @@ struct SharedMemoryCommand
|
||||
struct ExternalForceArgs m_externalForceArguments;
|
||||
struct CalculateInverseDynamicsArgs m_calculateInverseDynamicsArguments;
|
||||
struct CalculateJacobianArgs m_calculateJacobianArguments;
|
||||
struct CreateJointArgs m_createJointArguments;
|
||||
struct UserConstraintArgs m_userConstraintArguments;
|
||||
struct RequestContactDataArgs m_requestContactPointArguments;
|
||||
struct RequestOverlappingObjectsArgs m_requestOverlappingObjectsArgs;
|
||||
struct RequestVisualShapeDataArgs m_requestVisualShapeDataArguments;
|
||||
@ -620,6 +631,7 @@ struct SharedMemoryStatus
|
||||
struct CalculateInverseKinematicsResultArgs m_inverseKinematicsResultArgs;
|
||||
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
|
||||
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
|
||||
struct UserConstraintResultArgs m_userConstraintResultArgs;
|
||||
};
|
||||
};
|
||||
|
||||
|
||||
@ -33,7 +33,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_CALCULATE_INVERSE_DYNAMICS,
|
||||
CMD_CALCULATE_INVERSE_KINEMATICS,
|
||||
CMD_CALCULATE_JACOBIAN,
|
||||
CMD_CREATE_JOINT,
|
||||
CMD_USER_CONSTRAINT,
|
||||
CMD_REQUEST_CONTACT_POINT_INFORMATION,
|
||||
CMD_REQUEST_AABB_OVERLAP,
|
||||
CMD_SAVE_WORLD,
|
||||
@ -104,6 +104,8 @@ enum EnumSharedMemoryServerStatus
|
||||
CMD_LOAD_TEXTURE_FAILED,
|
||||
CMD_USER_DEBUG_DRAW_COMPLETED,
|
||||
CMD_USER_DEBUG_DRAW_FAILED,
|
||||
CMD_USER_CONSTRAINT_COMPLETED,
|
||||
CMD_USER_CONSTRAINT_FAILED,
|
||||
//don't go beyond 'CMD_MAX_SERVER_COMMANDS!
|
||||
CMD_MAX_SERVER_COMMANDS
|
||||
};
|
||||
|
||||
@ -263,7 +263,8 @@ static PyObject* pybullet_saveBullet(PyObject* self, PyObject* args)
|
||||
}
|
||||
command = b3SaveBulletCommandInit(sm, bulletFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
if (statusHandle != CMD_BULLET_SAVING_COMPLETED)
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_BULLET_SAVING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't save .bullet file.");
|
||||
return NULL;
|
||||
@ -291,7 +292,8 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args)
|
||||
}
|
||||
command = b3LoadMJCFCommandInit(sm, mjcfjFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
if (statusHandle != CMD_MJCF_LOADING_COMPLETED)
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType != CMD_MJCF_LOADING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Couldn't load .mjcf file.");
|
||||
return NULL;
|
||||
@ -1433,7 +1435,7 @@ static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) {
|
||||
}
|
||||
|
||||
// vector - double[3] which will be set by values from obVec
|
||||
static int pybullet_internalSetVector4(PyObject* obVec, double vector[4]) {
|
||||
static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) {
|
||||
int i, len;
|
||||
PyObject* seq;
|
||||
if (obVec==NULL)
|
||||
@ -1459,7 +1461,6 @@ static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObj
|
||||
int size = PySequence_Size(args);
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int res = 0;
|
||||
@ -1526,7 +1527,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
|
||||
int size = PySequence_Size(args);
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int res = 0;
|
||||
@ -1594,7 +1594,6 @@ static PyObject* pybullet_addUserDebugLine(PyObject* self, PyObject* args, PyObj
|
||||
static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int itemUniqueId;
|
||||
@ -1621,7 +1620,6 @@ static PyObject* pybullet_removeUserDebugItem(PyObject* self, PyObject* args, Py
|
||||
static PyObject* pybullet_removeAllUserDebugItems(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
struct b3ContactInformation contactPointData;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
@ -2016,6 +2014,144 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
static char *kwlist[] = { "userConstraintUniqueId",NULL};
|
||||
int userConstraintUniqueId=-1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
|
||||
|
||||
if (0 == sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i", kwlist,&userConstraintUniqueId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
commandHandle = b3InitRemoveUserConstraintCommand(sm,userConstraintUniqueId);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
};
|
||||
|
||||
|
||||
/*
|
||||
static PyObject* pybullet_updateUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
*/
|
||||
|
||||
static PyObject* pybullet_createUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
int size = PySequence_Size(args);
|
||||
int bodyUniqueIdA = -1;
|
||||
int bodyUniqueIdB = -1;
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
int parentBodyUniqueId=-1;
|
||||
int parentLinkIndex=-1;
|
||||
int childBodyUniqueId=-1;
|
||||
int childLinkIndex=-1;
|
||||
int jointType=ePoint2PointType;
|
||||
PyObject* jointAxisObj=0;
|
||||
double jointAxis[3]={0,0,0};
|
||||
PyObject* parentFramePositionObj = 0;
|
||||
double parentFramePosition[3]={0,0,0};
|
||||
PyObject* childFramePositionObj = 0;
|
||||
double childFramePosition[3]={0,0,0};
|
||||
PyObject* parentFrameOrientationObj = 0;
|
||||
double parentFrameOrientation[4]={0,0,0,1};
|
||||
PyObject* childFrameOrientationObj = 0;
|
||||
double childFrameOrientation[4]={0,0,0,1};
|
||||
|
||||
struct b3JointInfo jointInfo;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
PyObject* pyResultList = 0;
|
||||
|
||||
|
||||
static char *kwlist[] = { "parentBodyUniqueId", "parentLinkIndex",
|
||||
"childBodyUniqueId", "childLinkIndex",
|
||||
"jointType",
|
||||
"jointAxis",
|
||||
"parentFramePosition",
|
||||
"childFramePosition",
|
||||
"parentFrameOrientation",
|
||||
"childFrameOrientation",
|
||||
NULL };
|
||||
|
||||
if (0 == sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiiiiOOO|OO", kwlist,&parentBodyUniqueId,&parentLinkIndex,
|
||||
&childBodyUniqueId,&childLinkIndex,
|
||||
&jointType,&jointAxisObj,
|
||||
&parentFramePositionObj,
|
||||
&childFramePositionObj,
|
||||
&parentFrameOrientationObj,
|
||||
&childFrameOrientationObj
|
||||
))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pybullet_internalSetVectord(jointAxisObj,jointAxis);
|
||||
pybullet_internalSetVectord(parentFramePositionObj,parentFramePosition);
|
||||
pybullet_internalSetVectord(childFramePositionObj,childFramePosition);
|
||||
pybullet_internalSetVector4d(parentFrameOrientationObj,parentFrameOrientation);
|
||||
pybullet_internalSetVector4d(childFrameOrientationObj,childFrameOrientation);
|
||||
|
||||
jointInfo.m_jointType = jointType;
|
||||
jointInfo.m_parentFrame[0] = parentFramePosition[0];
|
||||
jointInfo.m_parentFrame[1] = parentFramePosition[1];
|
||||
jointInfo.m_parentFrame[2] = parentFramePosition[2];
|
||||
jointInfo.m_parentFrame[3] = parentFrameOrientation[0];
|
||||
jointInfo.m_parentFrame[4] = parentFrameOrientation[1];
|
||||
jointInfo.m_parentFrame[5] = parentFrameOrientation[2];
|
||||
jointInfo.m_parentFrame[6] = parentFrameOrientation[3];
|
||||
|
||||
jointInfo.m_childFrame[0] = childFramePosition[0];
|
||||
jointInfo.m_childFrame[1] = childFramePosition[1];
|
||||
jointInfo.m_childFrame[2] = childFramePosition[2];
|
||||
jointInfo.m_childFrame[3] = childFrameOrientation[0];
|
||||
jointInfo.m_childFrame[4] = childFrameOrientation[1];
|
||||
jointInfo.m_childFrame[5] = childFrameOrientation[2];
|
||||
jointInfo.m_childFrame[6] = childFrameOrientation[3];
|
||||
|
||||
jointInfo.m_jointAxis[0] = jointAxis[0];
|
||||
jointInfo.m_jointAxis[1] = jointAxis[1];
|
||||
jointInfo.m_jointAxis[2] = jointAxis[2];
|
||||
|
||||
commandHandle = b3InitCreateUserConstraintCommand(sm, parentBodyUniqueId, parentLinkIndex, childBodyUniqueId, childLinkIndex, &jointInfo);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType==CMD_USER_CONSTRAINT_COMPLETED)
|
||||
{
|
||||
int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
|
||||
PyObject* ob = PyLong_FromLong(userConstraintUid);
|
||||
return ob;
|
||||
} else
|
||||
{
|
||||
PyErr_SetString(SpamError, "createConstraint failed.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, PyObject *keywds) {
|
||||
int size = PySequence_Size(args);
|
||||
@ -2594,7 +2730,7 @@ static PyObject* pybullet_calculateInverseKinematicsKuka(PyObject* self,
|
||||
double pos[3];
|
||||
double ori[4]={0,1.0,0,0};
|
||||
|
||||
if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4(targetOrnObj,ori))
|
||||
if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori))
|
||||
{
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int numPos=0;
|
||||
@ -2820,6 +2956,14 @@ static PyMethodDef SpamMethods[] = {
|
||||
{ "loadMJCF", pybullet_loadMJCF, METH_VARARGS,
|
||||
"Load multibodies from an MJCF file." },
|
||||
|
||||
{"createConstraint", (PyCFunction)pybullet_createUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
||||
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
|
||||
},
|
||||
|
||||
{"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
||||
"Remove a constraint using its unique id."
|
||||
},
|
||||
|
||||
{"saveWorld", pybullet_saveWorld, METH_VARARGS,
|
||||
"Save a approximate Python file to reproduce the current state of the world: saveWorld"
|
||||
"(filename). (very preliminary and approximately)"},
|
||||
|
||||
35
examples/pybullet/quadruped.py
Normal file
35
examples/pybullet/quadruped.py
Normal file
@ -0,0 +1,35 @@
|
||||
import pybullet as p
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("plane.urdf")
|
||||
p.loadURDF("quadruped/quadruped.urdf",0,0,0.2)
|
||||
#p.getNumJoints(1)
|
||||
|
||||
#right front leg
|
||||
p.resetJointState(1,0,1.57)
|
||||
p.resetJointState(1,2,-2.2)
|
||||
p.resetJointState(1,3,-1.57)
|
||||
p.resetJointState(1,5,2.2)
|
||||
p.createConstraint(1,2,1,5,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
|
||||
#left front leg
|
||||
p.resetJointState(1,6,1.57)
|
||||
p.resetJointState(1,8,-2.2)
|
||||
p.resetJointState(1,9,-1.57)
|
||||
p.resetJointState(1,11,2.2)
|
||||
p.createConstraint(1,8,1,11,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
|
||||
#right back leg
|
||||
p.resetJointState(1,12,1.57)
|
||||
p.resetJointState(1,14,-2.2)
|
||||
p.resetJointState(1,15,-1.57)
|
||||
p.resetJointState(1,17,2.2)
|
||||
p.createConstraint(1,14,1,17,3,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
|
||||
|
||||
#left back leg
|
||||
p.resetJointState(1,18,1.57)
|
||||
p.resetJointState(1,20,-2.2)
|
||||
p.resetJointState(1,21,-1.57)
|
||||
p.resetJointState(1,23,2.2)
|
||||
p.createConstraint(1,20,1,23,3,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user