Minimal code to retrieve the joints transforms/states of a model

[mohdjawadi]

Hi! Please I am using a custom model visualization/simulation framework based on C# WPF viewport3D . I would like to kindly ask how i can retrieve the state of each joint transformation so that I can reproduce the joint states in the front end. I am little bit confused on the methods getJoint(), getFrames(), and others. Does not know which one will actually return the current state of a joint. I know getOperationalPosition() only return return the state of the tool. I would like to know how to retrieve the states of the remaining joints.

Thanks

[rickertm]

I assume you are using the rl::mdl part of the library? The function Model::getPosition() will return a vector with the current position values for each joint. For the visualization, you are probably looking for the transformation matrices of the bodies/links of the model. These can be retrieved via Model::getBodyFrame(const ::std::size_t& i), where i is a value within the range 0 to Model::getBodies().

The following lines in the rlCoachMdl demo show how this can be used to synchronize the kinematic and visual models after a position update:

rl/demos/rlCoachMdl/ConfigurationModel.cpp

Lines 221 to 227 in 2da1f81

	kinematic->setPosition(q);
	kinematic->forwardPosition();
	for (std::size_t i = 0; i < MainWindow::instance()->geometryModels[this->id]->getNumBodies(); ++i)
	{
	MainWindow::instance()->geometryModels[this->id]->getBody(i)->setFrame(kinematic->getBodyFrame(i));
	}

[mohdjawadi]

Thank you for responding. I have retrieve the transformation matrix using the getBodyFrame() function but could not determine how to use this for the visualization. One major problem is determining joint origins that the a link will rotate about. Also, based on the rl::mdl::UrdfFactory codes, the joint origin has been extracted from the urdf file but added as a separate frame. So please could you assist me with the following:
(1) How to interpret the retrieved transformation matrix, like for example, to translate to Windows Presentation Foundation Matrix3D class
(2) How can I retrieve the joint origin translational and rotational offset so I can properly visualize the motion by performing the transformation based on this offset.
Thanks

[rickertm]

The transformation matrices are a typedef of Eigen::Transform. You can use Eigen::Transform::matrix() to get a 4x4 matrix that you can convert to the Matrix3D class.
The class rl::mdl::Joint is a subclass of rl::mdl::Transform and performs a transformation of Frame* in to Frame* out, both are public members. You can access each joint via Model::getJoint(const ::std::size_t& i).

[mohdjawadi]

Ok thank you I would try that and report back here.