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

Expose motor drive torque reporting for motors in spherical joints in getJointStateMultiDof.

Browse Source
This commit is contained in:
erwincoumans
2019-02-27 09:54:12 -08:00
parent 09dbb8ba1b
commit 8e1c1448ab
6 changed files with 291 additions and 299 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1047,6 +1047,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
for (int i = 0; i < state->m_uDofSize; i++)
{
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i];
state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_jointMotorForceMultiDof[info.m_uIndex + i];
}
}
else
@@ -1058,7 +1059,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
{
state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
return 1;
}
}

26
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6425,6 +6425,32 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
}
for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
{
serverCmd.m_sendActualStateArgs.m_jointMotorForce[totalDegreeOfFreedomU] = 0;
if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical)
{
btMultiBodySphericalJointMotor* motor = (btMultiBodySphericalJointMotor*)mb->getLink(l).m_userPtr;
if (motor)
{
btScalar impulse = motor->getAppliedImpulse(d);
btScalar force = impulse / m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
}
}
else
{
if (supportsJointMotor(mb, l))
{
btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr;
if (motor && m_data->m_physicsDeltaTime > btScalar(0))
{
btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
}
}
}
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}

1
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -531,6 +531,7 @@ struct SendActualStateArgs
double m_jointReactionForces[6 * MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
double m_jointMotorForceMultiDof[MAX_DEGREE_OF_FREEDOM];
double m_linkState[7 * MAX_NUM_LINKS];
double m_linkWorldVelocities[6 * MAX_NUM_LINKS]; //linear velocity and angular velocity in world space (x/y/z each).

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -369,7 +369,7 @@ 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;
double m_jointMotorTorqueMultiDof[3];
int m_qDofSize;
int m_uDofSize;
};