The output signal of RigidBodySystem is defined as follows (see RigidbodySystem:141):
template <typename ScalarType>
using OutputVector = Eigen::Matrix<ScalarType, Eigen::Dynamic, 1>;
In the context of the the car simulation, ScalarType is a double. Unfortunately, a vector of doubles does not provide any information about the meaning of the output values.
The first part of the output vector consists of the positions and velocities of the joints in the RigidBodyTree. The location within the output vector that each of these states is stored can be derived from member variables within the RigidBody objects that make up the RigidBodyTree. These member variables are initialized in RigidBodyTree::compile(). The relevant code starts on RigidBodyTree:131:
num_positions = 0;
num_velocities = 0;
for (auto it = bodies.begin(); it != bodies.end(); ++it) {
RigidBody& body = **it;
if (body.hasParent()) {
body.position_num_start = num_positions;
num_positions += body.getJoint().getNumPositions();
body.velocity_num_start = num_velocities;
num_velocities += body.getJoint().getNumVelocities();
} else {
body.position_num_start = 0;
body.velocity_num_start = 0;
}
}
Using the variables RigidBodyTree::num_positions, RigidBodyTree::num_velocities, RigidBody::position_num_start, and RigidBody::velocity_num_start the current position and velocities of each DOF can be extracted from the output vector of RigidBodySystem.
The remaining part of RigidBodySystem's output vector consists of sensor states. Unfortunately, it is currently not possible to determine the semantics of this data. There's a System Framework 2.0 design that should eventually resolve this problem, but we need a short term solution in the mean time. I created this issue to track the changes necessary to a consumer of RigidBodySystem's output can extract the sensor state contained within this output.
The output signal of
RigidBodySystemis defined as follows (see RigidbodySystem:141):In the context of the the car simulation,
ScalarTypeis adouble. Unfortunately, a vector of doubles does not provide any information about the meaning of the output values.The first part of the output vector consists of the positions and velocities of the joints in the
RigidBodyTree. The location within the output vector that each of these states is stored can be derived from member variables within theRigidBodyobjects that make up theRigidBodyTree. These member variables are initialized inRigidBodyTree::compile(). The relevant code starts on RigidBodyTree:131:Using the variables
RigidBodyTree::num_positions,RigidBodyTree::num_velocities,RigidBody::position_num_start, andRigidBody::velocity_num_startthe current position and velocities of each DOF can be extracted from the output vector ofRigidBodySystem.The remaining part of
RigidBodySystem's output vector consists of sensor states. Unfortunately, it is currently not possible to determine the semantics of this data. There's a System Framework 2.0 design that should eventually resolve this problem, but we need a short term solution in the mean time. I created this issue to track the changes necessary to a consumer of RigidBodySystem's output can extract the sensor state contained within this output.