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');
replaceJoint(robot3DoF_loaded, 'link2_ssrms', fixedJnt_Link1ToLink2);
fixedJnt_Link2ToLink3 = rigidBodyJoint('shoulder_yaw_fixed','fixed');
replaceJoint(robot3DoF_loaded, 'link3_ssrms', fixedJnt_Link2ToLink3);
fixedJnt_Link3ToLink4 = rigidBodyJoint('elbow_yaw_fixed','fixed');
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.