When I change joint type to fixed, it still appears in the robot configuration with NaN value. Why does it happens?

Hello everybody,

I'm trying to reduce my 7DoF manipulator to a 4DoF one in order to control it by a simpler adaptive controller (A 7DoF symbolic Dynamic Regressor is very expensive to compute and evaluate). So I computed the inverse kinemaics, got the final configuration of all my 7 joints and got the proper transforms between each rigidBody I want keep fixed. Then I created the joints, I set them with the proper transform and I replaced them in their rigidBodies.

All seems to be right, but when I ask a random configuration, MATLAB returns an array of 1x7 with three values set as NaN and not a proper 1x4 array. Even the showdetails command returns me all right.

Here below I attach my code for the first three links that I want to fix each other:

robot3DoF_loaded = copy(robot7DoF_loaded);

fixedJnt_Link1ToLink2 = rigidBodyJoint('shoulder_roll_fixed','fixed');
setFixedTransform(fixedJnt_Link1ToLink2, inv(T_Link1ToLink2));
replaceJoint(robot3DoF_loaded, 'link2_ssrms', fixedJnt_Link1ToLink2);

fixedJnt_Link2ToLink3 = rigidBodyJoint('shoulder_yaw_fixed','fixed');
setFixedTransform(fixedJnt_Link2ToLink3, inv(T_Link2ToLink3));
replaceJoint(robot3DoF_loaded, 'link3_ssrms', fixedJnt_Link2ToLink3);

fixedJnt_Link3ToLink4 = rigidBodyJoint('elbow_yaw_fixed','fixed');
setFixedTransform(fixedJnt_Link3ToLink4, inv(T_Link3ToLink4));
replaceJoint(robot3DoF_loaded, 'link3_ssrms', fixedJnt_Link3ToLink4);

Here below I attach what MATLAB returns me

q_random3DoF = randomConfiguration(robot3DoF_loaded) = 
    [NaN NaN NaN 0.158 1.458 -0.985 0.052] 

Warning: Rigid body tree dynamics method returned invalid (NaN or Inf) results. Please check the
inertial parameters of the rigidBodyTree object. For example, a leaf non-fixed body that has zero
mass can cause forwardDynamics to return NaN. 
> In robotics.manip.internal.warning (line 19)
In robotics.manip.internal/RigidBodyTree/resultPostProcess (line 1303)
In robotics.manip.internal/RigidBodyTree/randomConfiguration (line 601)
In rigidBodyTree/randomConfiguration (line 450)

PS: I controlled all inertial parameter but they're not zero, except for the end-effector but it was and is still fixed to the last link.

[Yiping Liu]

Can you share your robot7DoF_loaded code so we can reproduce the issue?

[Valerio Novelli]

Thank for reply me.

Sure but I didn't build it in MATLAB, I imported it from an urdf file which I created by the tool "sw2urdf". It doesn't have any issue in its 7DoF form but when I change some joints from revolute to fixed (specified by their proper setFixedTransform), problems come out.

I set the fullfilepath of the urdf file in the function import robot which you can find in the attached below.

I found a workaround in Solidworks to create the reduced DOF robot but this method is surely more efficient and elegant.

Who was interested to my workaround, I built directly a DoF-reduced robot model in urdf format, then I replaced fixed joints (not in their proper configuration) with other fixed joints that i properly set with their setFixedTransform. In this case, all works.

Answer 1

[Yiping Liu]

Hi Valerio,

So this turns out to be a bug in the replaceJoint method (some internal numbers are not updated correctly when you convert a revolute joint into a fixed joint). Sorry for the inconvenience if this bug has blocked your workflow. replaceJoint is one of the lesser used methods on rigidBodyTree, thanks for spotting it. Please reach out directly to the MathWorks tech support and mentioning this post. The dev team can get a hot patch for you in one or two days based on the version of MATLAB you have.

Thanks,

Yiping