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

Merge pull request #2130 from erwincoumans/master

Browse Source 
Expose motor drive torque reporting for motors in spherical joints
This commit is contained in:
erwincoumans
2019-02-27 11:53:06 -08:00
committed by GitHub
parent ebc079c32c 2ae03f50b1
commit dfa66864a3
6 changed files with 175 additions and 135 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;
};

8
examples/pybullet/examples/humanoidMotionCapture.py
View File

@@ -10,6 +10,7 @@ else:
useZUp = False
useYUp = not useZUp
showJointMotorTorques = False
if useYUp:
p.configureDebugVisualizer(p.COV_ENABLE_Y_AXIS_UP , 1)
@@ -428,4 +429,11 @@ while (p.isConnected()):
p.resetJointStateMultiDof(humanoid4,leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid4,leftElbow, leftElbowRot)
p.stepSimulation()
if showJointMotorTorques:
for j in range(p.getNumJoints(humanoid2)):
jointState = p.getJointStateMultiDof(humanoid2,j)
print("jointStateMultiDof[",j,"].pos=",jointState[0])
print("jointStateMultiDof[",j,"].vel=",jointState[1])
print("jointStateMultiDof[",j,"].jointForces=",jointState[3])
time.sleep(timeStep)

10
examples/pybullet/pybullet.c
View File

@@ -4235,6 +4235,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyObject* pyListJointState;
PyObject* pyListPosition;
PyObject* pyListVelocity;
PyObject* pyListJointMotorTorque;
struct b3JointSensorState2 sensorState;
@@ -4296,7 +4297,7 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
int i = 0;
pyListPosition = PyTuple_New(sensorState.m_qDofSize);
pyListVelocity = PyTuple_New(sensorState.m_uDofSize);
pyListJointMotorTorque = PyTuple_New(sensorState.m_uDofSize);
PyTuple_SetItem(pyListJointState, 0, pyListPosition);
PyTuple_SetItem(pyListJointState, 1, pyListVelocity);
@@ -4310,6 +4311,9 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
{
PyTuple_SetItem(pyListVelocity, i,
PyFloat_FromDouble(sensorState.m_jointVelocity[i]));
PyTuple_SetItem(pyListJointMotorTorque, i,
PyFloat_FromDouble(sensorState.m_jointMotorTorqueMultiDof[i]));
}
for (j = 0; j < forceTorqueSize; j++)
@@ -4320,8 +4324,8 @@ static PyObject* pybullet_getJointStateMultiDof(PyObject* self, PyObject* args,
PyTuple_SetItem(pyListJointState, 2, pyListJointForceTorque);
PyTuple_SetItem(pyListJointState, 3,
PyFloat_FromDouble(sensorState.m_jointMotorTorque));
PyTuple_SetItem(pyListJointState, 3, pyListJointMotorTorque);
return pyListJointState;
}