mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 21:30:09 +00:00
enable motor control for maximal coordinates robots (btRigidBody, btTypedConstraint) for force, velocity, position control.
This commit is contained in:
erwincoumans
parent
4ff6befc6d
commit
1f7db4519e
@ -180,7 +180,7 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int u
|
||||
|
||||
dof6->setAngularLowerLimit(btVector3(0,0,0));
|
||||
dof6->setAngularUpperLimit(btVector3(0,0,0));
|
||||
|
||||
m_6DofConstraints.push_back(dof6);
|
||||
return dof6;
|
||||
}
|
||||
|
||||
@ -188,6 +188,10 @@ class btGeneric6DofSpring2Constraint* MyMultiBodyCreator::createFixedJoint(int u
|
||||
|
||||
void MyMultiBodyCreator::addLinkMapping(int urdfLinkIndex, int mbLinkIndex)
|
||||
{
|
||||
if (m_mb2urdfLink.size()<(mbLinkIndex+1))
|
||||
{
|
||||
m_mb2urdfLink.resize((mbLinkIndex+1),-2);
|
||||
}
|
||||
// m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
|
||||
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
|
||||
}
|
||||
|
||||
@ -378,12 +378,14 @@ void ConvertURDF2BulletInternal(
|
||||
{
|
||||
printf("Warning: joint unsupported, creating a fixed joint instead.");
|
||||
}
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
//todo: adjust the center of mass transform and pivot axis properly
|
||||
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, offsetInA.getOrigin(),-offsetInB.getOrigin());
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
} else
|
||||
{
|
||||
//b3Printf("Fixed joint\n");
|
||||
@ -398,13 +400,14 @@ void ConvertURDF2BulletInternal(
|
||||
case URDFContinuousJoint:
|
||||
case URDFRevoluteJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupRevolute(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (jointType == URDFRevoluteJoint && jointLowerLimit <= jointUpperLimit) {
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
//printf("create btMultiBodyJointLimitConstraint for revolute link name=%s urdf link index=%d (low=%f, up=%f)\n", name.c_str(), urdfLinkIndex, jointLowerLimit, jointUpperLimit);
|
||||
@ -424,13 +427,15 @@ void ConvertURDF2BulletInternal(
|
||||
}
|
||||
case URDFPrismaticJoint:
|
||||
{
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (createMultiBody)
|
||||
{
|
||||
cache.m_bulletMultiBody->setupPrismatic(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
|
||||
parentRotToThis, quatRotate(offsetInB.getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
|
||||
-offsetInB.getOrigin(),
|
||||
disableParentCollision);
|
||||
creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
|
||||
|
||||
if (jointLowerLimit <= jointUpperLimit)
|
||||
{
|
||||
//std::string name = u2b.getLinkName(urdfLinkIndex);
|
||||
@ -444,7 +449,8 @@ void ConvertURDF2BulletInternal(
|
||||
} else
|
||||
{
|
||||
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
|
||||
//btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
|
||||
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
|
||||
|
||||
|
||||
if (enableConstraints)
|
||||
|
||||
@ -278,7 +278,10 @@ void PhysicsClientSharedMemory::disconnectSharedMemory() {
|
||||
|
||||
}
|
||||
|
||||
bool PhysicsClientSharedMemory::isConnected() const { return m_data->m_isConnected; }
|
||||
bool PhysicsClientSharedMemory::isConnected() const
|
||||
{
|
||||
return m_data->m_isConnected && (m_data->m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER);
|
||||
}
|
||||
|
||||
bool PhysicsClientSharedMemory::connect() {
|
||||
/// server always has to create and initialize shared memory
|
||||
@ -386,6 +389,72 @@ void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const Sha
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T, typename U> void addJointInfoFromConstraint(int linkIndex, const T* con, U* bodyJoints, bool verboseOutput)
|
||||
{
|
||||
b3JointInfo info;
|
||||
info.m_jointName = 0;
|
||||
info.m_linkName = 0;
|
||||
info.m_flags = 0;
|
||||
info.m_jointIndex = linkIndex;
|
||||
info.m_qIndex = linkIndex+6;
|
||||
info.m_uIndex = linkIndex+6;
|
||||
//derive type from limits
|
||||
|
||||
if (con->m_typeConstraintData.m_name)
|
||||
{
|
||||
info.m_jointName = strDup(con->m_typeConstraintData.m_name);
|
||||
//info.m_linkName = strDup(con->m_typeConstraintData.m_name);
|
||||
}
|
||||
|
||||
btVector3 linearLowerLimit(con->m_linearLowerLimit.m_floats[0],con->m_linearLowerLimit.m_floats[1],con->m_linearLowerLimit.m_floats[2]);
|
||||
btVector3 linearUpperLimit(con->m_linearUpperLimit.m_floats[0],con->m_linearUpperLimit.m_floats[1],con->m_linearUpperLimit.m_floats[2]);
|
||||
btVector3 angularLowerLimit(con->m_angularLowerLimit.m_floats[0],con->m_angularLowerLimit.m_floats[1],con->m_angularLowerLimit.m_floats[2]);
|
||||
btVector3 angularUpperLimit(con->m_angularUpperLimit.m_floats[0],con->m_angularUpperLimit.m_floats[1],con->m_angularUpperLimit.m_floats[2]);
|
||||
|
||||
//very simple, rudimentary extraction of constaint type, from limits
|
||||
info.m_jointType = eFixedType;
|
||||
info.m_jointDamping = 0;//mb->m_links[link].m_jointDamping;
|
||||
info.m_jointFriction = 0;//mb->m_links[link].m_jointFriction;
|
||||
info.m_jointLowerLimit = 0;//mb->m_links[link].m_jointLowerLimit;
|
||||
info.m_jointUpperLimit = 0;//mb->m_links[link].m_jointUpperLimit;
|
||||
info.m_jointMaxForce = 0;//mb->m_links[link].m_jointMaxForce;
|
||||
info.m_jointMaxVelocity = 0;//mb->m_links[link].m_jointMaxVelocity;
|
||||
|
||||
|
||||
if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero())
|
||||
{
|
||||
info.m_jointType = eFixedType;
|
||||
} else
|
||||
{
|
||||
if (linearLowerLimit.isZero() && linearUpperLimit.isZero())
|
||||
{
|
||||
info.m_jointType = eRevoluteType;
|
||||
btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute();
|
||||
int limitAxis = limitRange.maxAxis();
|
||||
info.m_jointLowerLimit = angularLowerLimit[limitAxis];
|
||||
info.m_jointUpperLimit = angularUpperLimit[limitAxis];
|
||||
} else
|
||||
{
|
||||
info.m_jointType = ePrismaticType;
|
||||
btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute();
|
||||
int limitAxis = limitRange.maxAxis();
|
||||
info.m_jointLowerLimit = linearLowerLimit[limitAxis];
|
||||
info.m_jointUpperLimit = linearUpperLimit[limitAxis];
|
||||
}
|
||||
}
|
||||
|
||||
//if (mb->m_links[link].m_linkName) {
|
||||
|
||||
|
||||
if ((info.m_jointType == eRevoluteType) ||
|
||||
(info.m_jointType == ePrismaticType)) {
|
||||
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
|
||||
}
|
||||
bodyJoints->m_jointInfo.push_back(info);
|
||||
|
||||
|
||||
};
|
||||
|
||||
const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
// SharedMemoryStatus* stat = 0;
|
||||
|
||||
@ -460,7 +529,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
b3Printf("Server loading the URDF OK\n");
|
||||
}
|
||||
|
||||
if (serverCmd.m_numDataStreamBytes > 0) {
|
||||
if (serverCmd.m_numDataStreamBytes > 0)
|
||||
{
|
||||
bParse::btBulletFile bf(
|
||||
this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor,
|
||||
serverCmd.m_numDataStreamBytes);
|
||||
@ -472,12 +542,31 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
|
||||
bodyJoints->m_bodyName = serverCmd.m_dataStreamArguments.m_bodyName;
|
||||
|
||||
for (int i = 0; i < bf.m_multiBodies.size(); i++) {
|
||||
for (int i = 0; i < bf.m_constraints.size(); i++)
|
||||
{
|
||||
int flag = bf.getFlags();
|
||||
|
||||
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
|
||||
{
|
||||
Bullet::btGeneric6DofSpring2ConstraintDoubleData2* con =
|
||||
(Bullet::btGeneric6DofSpring2ConstraintDoubleData2*) bf.m_constraints[i];
|
||||
addJointInfoFromConstraint(i,con,bodyJoints,m_data->m_verboseOutput);
|
||||
} else
|
||||
{
|
||||
Bullet::btGeneric6DofSpring2ConstraintData* con =
|
||||
(Bullet::btGeneric6DofSpring2ConstraintData*) bf.m_constraints[i];
|
||||
addJointInfoFromConstraint(i,con,bodyJoints,m_data->m_verboseOutput);
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < bf.m_multiBodies.size(); i++)
|
||||
{
|
||||
|
||||
|
||||
int flag = bf.getFlags();
|
||||
|
||||
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0) {
|
||||
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
|
||||
{
|
||||
Bullet::btMultiBodyDoubleData* mb =
|
||||
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
|
||||
if (mb->m_baseName)
|
||||
|
||||
@ -76,6 +76,8 @@ btScalar gRhsClamp = 1.f;
|
||||
struct UrdfLinkNameMapUtil
|
||||
{
|
||||
btMultiBody* m_mb;
|
||||
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
|
||||
|
||||
btDefaultSerializer* m_memSerializer;
|
||||
|
||||
UrdfLinkNameMapUtil():m_mb(0),m_memSerializer(0)
|
||||
@ -148,6 +150,10 @@ struct InternalBodyData
|
||||
|
||||
btTransform m_rootLocalInertialFrame;
|
||||
btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
|
||||
btAlignedObjectArray<btGeneric6DofSpring2Constraint*> m_rigidBodyJoints;
|
||||
btAlignedObjectArray<std::string> m_rigidBodyJointNames;
|
||||
btAlignedObjectArray<std::string> m_rigidBodyLinkNames;
|
||||
|
||||
#ifdef B3_ENABLE_TINY_AUDIO
|
||||
b3HashMap<btHashInt, SDFAudioSource> m_audioSources;
|
||||
#endif //B3_ENABLE_TINY_AUDIO
|
||||
@ -165,6 +171,9 @@ struct InternalBodyData
|
||||
m_bodyName="";
|
||||
m_rootLocalInertialFrame.setIdentity();
|
||||
m_linkLocalInertialFrames.clear();
|
||||
m_rigidBodyJoints.clear();
|
||||
m_rigidBodyJointNames.clear();
|
||||
m_rigidBodyLinkNames.clear();
|
||||
}
|
||||
|
||||
};
|
||||
@ -2235,6 +2244,32 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
|
||||
bodyHandle->m_rigidBody = rb;
|
||||
rb->setUserIndex2(bodyUniqueId);
|
||||
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
|
||||
|
||||
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
||||
m_data->m_strings.push_back(baseName);
|
||||
bodyHandle->m_bodyName = *baseName;
|
||||
|
||||
int numJoints = creation.getNum6DofConstraints();
|
||||
bodyHandle->m_rigidBodyJoints.reserve(numJoints);
|
||||
bodyHandle->m_rigidBodyJointNames.reserve(numJoints);
|
||||
bodyHandle->m_rigidBodyLinkNames.reserve(numJoints);
|
||||
for (int i=0;i<numJoints;i++)
|
||||
{
|
||||
int urdfLinkIndex = creation.m_mb2urdfLink[i];
|
||||
|
||||
btGeneric6DofSpring2Constraint* con = creation.get6DofConstraint(i);
|
||||
|
||||
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
|
||||
m_data->m_strings.push_back(linkName);
|
||||
|
||||
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
|
||||
m_data->m_strings.push_back(jointName);
|
||||
|
||||
bodyHandle->m_rigidBodyJointNames.push_back(*jointName);
|
||||
bodyHandle->m_rigidBodyLinkNames.push_back(*linkName);
|
||||
|
||||
bodyHandle->m_rigidBodyJoints.push_back(con);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
@ -2594,7 +2629,64 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
|
||||
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
|
||||
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
|
||||
|
||||
} else
|
||||
{
|
||||
btRigidBody* rb = bodyHandle? bodyHandle->m_rigidBody :0;
|
||||
if (rb)
|
||||
{
|
||||
UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
|
||||
m_data->m_urdfLinkNameMapper.push_back(util);
|
||||
util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
|
||||
util->m_memSerializer->startSerialization();
|
||||
util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody,bodyHandle->m_bodyName.c_str());
|
||||
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
|
||||
for (int i=0;i<bodyHandle->m_rigidBodyJoints.size();i++)
|
||||
{
|
||||
const btGeneric6DofSpring2Constraint* con = bodyHandle->m_rigidBodyJoints.at(i);
|
||||
#if 0
|
||||
const btRigidBody& bodyA = con->getRigidBodyA();
|
||||
const btRigidBody& bodyB = con->getRigidBodyB();
|
||||
int len = bodyA.calculateSerializeBufferSize();
|
||||
btChunk* chunk = util->m_memSerializer->allocate(len,1);
|
||||
const char* structType = bodyA.serialize(chunk->m_oldPtr, util->m_memSerializer);
|
||||
util->m_memSerializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)&bodyA);
|
||||
#endif
|
||||
util->m_memSerializer->registerNameForPointer(con,bodyHandle->m_rigidBodyJointNames[i].c_str());
|
||||
util->m_memSerializer->registerNameForPointer(&con->getRigidBodyB(),bodyHandle->m_rigidBodyLinkNames[i].c_str());
|
||||
|
||||
const btRigidBody& bodyA = con->getRigidBodyA();
|
||||
|
||||
int len = con->calculateSerializeBufferSize();
|
||||
btChunk* chunk = util->m_memSerializer->allocate(len,1);
|
||||
const char* structType = con->serialize(chunk->m_oldPtr, util->m_memSerializer);
|
||||
util->m_memSerializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,(void*)con);
|
||||
}
|
||||
|
||||
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
|
||||
#if 0
|
||||
util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
|
||||
if (mb->getBaseName())
|
||||
{
|
||||
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
|
||||
}
|
||||
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
|
||||
for (int i=0;i<mb->getNumLinks();i++)
|
||||
{
|
||||
//disable serialization of the collision objects
|
||||
util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
|
||||
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_linkName,mb->getLink(i).m_linkName);
|
||||
util->m_memSerializer->registerNameForPointer(mb->getLink(i).m_jointName,mb->getLink(i).m_jointName);
|
||||
}
|
||||
util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName());
|
||||
int len = mb->calculateSerializeBufferSize();
|
||||
btChunk* chunk = util->m_memSerializer->allocate(len,1);
|
||||
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
|
||||
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
|
||||
streamSizeInBytes = util->m_memSerializer->getCurrentBufferSize();
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
return streamSizeInBytes;
|
||||
}
|
||||
|
||||
@ -4438,6 +4530,165 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
}
|
||||
|
||||
}
|
||||
} else
|
||||
{
|
||||
//support for non-btMultiBody, such as btRigidBody
|
||||
|
||||
if (body && body->m_rigidBody)
|
||||
{
|
||||
btRigidBody* rb = body->m_rigidBody;
|
||||
btAssert(rb);
|
||||
|
||||
//switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
|
||||
{
|
||||
//case CONTROL_MODE_TORQUE:
|
||||
{
|
||||
if (m_data->m_verboseOutput)
|
||||
{
|
||||
b3Printf("Using CONTROL_MODE_TORQUE");
|
||||
}
|
||||
// mb->clearForcesAndTorques();
|
||||
int torqueIndex = 6;
|
||||
//if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
|
||||
{
|
||||
for (int link=0;link<body->m_rigidBodyJoints.size();link++)
|
||||
{
|
||||
btGeneric6DofSpring2Constraint* con = body->m_rigidBodyJoints[link];
|
||||
|
||||
btVector3 linearLowerLimit;
|
||||
btVector3 linearUpperLimit;
|
||||
btVector3 angularLowerLimit;
|
||||
btVector3 angularUpperLimit;
|
||||
|
||||
|
||||
//for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
|
||||
{
|
||||
double torque = 0.f;
|
||||
//if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
|
||||
{
|
||||
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
|
||||
btScalar qdotTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[torqueIndex];
|
||||
btScalar qTarget = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[torqueIndex];
|
||||
|
||||
|
||||
con->getLinearLowerLimit(linearLowerLimit);
|
||||
con->getLinearUpperLimit(linearUpperLimit);
|
||||
con->getAngularLowerLimit(angularLowerLimit);
|
||||
con->getAngularUpperLimit(angularUpperLimit);
|
||||
|
||||
if (linearLowerLimit.isZero() && linearUpperLimit.isZero() && angularLowerLimit.isZero() && angularUpperLimit.isZero())
|
||||
{
|
||||
//fixed, don't do anything
|
||||
} else
|
||||
{
|
||||
con->calculateTransforms();
|
||||
|
||||
if (linearLowerLimit.isZero() && linearUpperLimit.isZero())
|
||||
{
|
||||
//eRevoluteType;
|
||||
btVector3 limitRange = angularLowerLimit.absolute()+angularUpperLimit.absolute();
|
||||
int limitAxis = limitRange.maxAxis();
|
||||
const btTransform& transA = con->getCalculatedTransformA();
|
||||
const btTransform& transB = con->getCalculatedTransformB();
|
||||
btVector3 axisA = transA.getBasis().getColumn(limitAxis);
|
||||
btVector3 axisB = transB.getBasis().getColumn(limitAxis);
|
||||
|
||||
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
|
||||
{
|
||||
case CONTROL_MODE_TORQUE:
|
||||
{
|
||||
con->getRigidBodyA().applyTorque(torque*axisA);
|
||||
con->getRigidBodyB().applyTorque(-torque*axisB);
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_VELOCITY:
|
||||
{
|
||||
con->enableMotor(3+limitAxis,true);
|
||||
con->setTargetVelocity(3+limitAxis, qdotTarget);
|
||||
//this is max motor force impulse
|
||||
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
|
||||
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_POSITION_VELOCITY_PD:
|
||||
{
|
||||
con->setServo(3+limitAxis,true);
|
||||
con->setServoTarget(3+limitAxis,qTarget);
|
||||
//next one is the maximum velocity to reach target position.
|
||||
//the maximum velocity is limited by maxMotorForce
|
||||
con->setTargetVelocity(3+limitAxis, 100);
|
||||
//this is max motor force impulse
|
||||
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
|
||||
con->setMaxMotorForce(3+limitAxis,torqueImpulse);
|
||||
con->enableMotor(3+limitAxis,true);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
} else
|
||||
{
|
||||
//ePrismaticType; @todo
|
||||
btVector3 limitRange = linearLowerLimit.absolute()+linearUpperLimit.absolute();
|
||||
int limitAxis = limitRange.maxAxis();
|
||||
|
||||
const btTransform& transA = con->getCalculatedTransformA();
|
||||
const btTransform& transB = con->getCalculatedTransformB();
|
||||
btVector3 axisA = transA.getBasis().getColumn(limitAxis);
|
||||
btVector3 axisB = transB.getBasis().getColumn(limitAxis);
|
||||
|
||||
switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode)
|
||||
{
|
||||
case CONTROL_MODE_TORQUE:
|
||||
{
|
||||
con->getRigidBodyA().applyForce(-torque*axisA,btVector3(0,0,0));
|
||||
con->getRigidBodyB().applyForce(torque*axisB,btVector3(0,0,0));
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_VELOCITY:
|
||||
{
|
||||
con->enableMotor(limitAxis,true);
|
||||
con->setTargetVelocity(limitAxis, -qdotTarget);
|
||||
//this is max motor force impulse
|
||||
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
|
||||
con->setMaxMotorForce(limitAxis,torqueImpulse);
|
||||
break;
|
||||
}
|
||||
case CONTROL_MODE_POSITION_VELOCITY_PD:
|
||||
{
|
||||
con->setServo(limitAxis,true);
|
||||
con->setServoTarget(limitAxis,qTarget);
|
||||
//next one is the maximum velocity to reach target position.
|
||||
//the maximum velocity is limited by maxMotorForce
|
||||
con->setTargetVelocity(limitAxis, 100);
|
||||
//this is max motor force impulse
|
||||
btScalar torqueImpulse = torque*m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
|
||||
con->setMaxMotorForce(limitAxis,torqueImpulse);
|
||||
con->enableMotor(limitAxis,true);
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
}
|
||||
}//fi
|
||||
torqueIndex++;
|
||||
}
|
||||
}
|
||||
}//fi
|
||||
//break;
|
||||
}
|
||||
|
||||
}
|
||||
} //if (body && body->m_rigidBody)
|
||||
}
|
||||
|
||||
serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED;
|
||||
@ -6937,7 +7188,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
int uid = m_data->m_visualConverter.loadTextureFile(relativeFileName);
|
||||
if(uid>=0)
|
||||
{
|
||||
int m_tinyRendererTextureId;
|
||||
texH->m_tinyRendererTextureId = uid;
|
||||
}
|
||||
|
||||
|
||||
@ -2827,8 +2827,17 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
|
||||
// info.m_jointDamping,
|
||||
// info.m_jointFriction);
|
||||
PyTuple_SetItem(pyListJointInfo, 0, PyInt_FromLong(info.m_jointIndex));
|
||||
|
||||
if (info.m_jointName)
|
||||
{
|
||||
PyTuple_SetItem(pyListJointInfo, 1,
|
||||
PyString_FromString(info.m_jointName));
|
||||
} else
|
||||
{
|
||||
PyTuple_SetItem(pyListJointInfo, 1,
|
||||
PyString_FromString("not available"));
|
||||
}
|
||||
|
||||
PyTuple_SetItem(pyListJointInfo, 2, PyInt_FromLong(info.m_jointType));
|
||||
PyTuple_SetItem(pyListJointInfo, 3, PyInt_FromLong(info.m_qIndex));
|
||||
PyTuple_SetItem(pyListJointInfo, 4, PyInt_FromLong(info.m_uIndex));
|
||||
@ -2845,8 +2854,16 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
|
||||
PyFloat_FromDouble(info.m_jointMaxForce));
|
||||
PyTuple_SetItem(pyListJointInfo, 11,
|
||||
PyFloat_FromDouble(info.m_jointMaxVelocity));
|
||||
if (info.m_linkName)
|
||||
{
|
||||
|
||||
PyTuple_SetItem(pyListJointInfo, 12,
|
||||
PyString_FromString(info.m_linkName));
|
||||
} else
|
||||
{
|
||||
PyTuple_SetItem(pyListJointInfo, 12,
|
||||
PyString_FromString("not available"));
|
||||
}
|
||||
|
||||
return pyListJointInfo;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user