search

robotology-legacy / mex-wholebodymodel

Matlab MEX interface to the iWholeBodyModel interface.
11 stars 9 forks source link

Semi Positive Definite Mass Matrix from URDF using mexWholeBodyModel #72

Closed yeshasvitirupachuri closed 3 years ago

yeshasvitirupachuri commented 8 years ago

Ciao,

I am trying to use mexWholeBodyModel with an URDF file for a 1R mechanism. Please check the code for it below. I also attached the Matlab Script used to load the urdf model using initializeFromURDF and get the parameters. But the Mass Matrix is not being positive definite. Please update on this

Thank you

<!--URDF CODE-->
<robot>
<link name="world"/>
  <!--Base Link is at the Origin-->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="5"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <!--Base Link Box-->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.3 0.05 0.05"/>
      </geometry>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.3 0.05 0.05"/>
      </geometry>
    </collision>
    <!--Base Link Small Box-->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.025 -0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
      </geometry>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.025 -0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
      </geometry>
    </collision>
    <!--Base Link Small Cylinder-->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.075 -0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
      </geometry>
      <material name="white">
        <color rgba="1.0 1.0 1.0 1.0"/>
      </material>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.075 -0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <!--First Link-->
  <link name="first_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0.1375 0"/>
      <mass value="2"/>
      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
    </inertial>
    <!--First Link Small Cylinder - Front -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
        <material name="white">
          <color rgba="1.0 1.0 1.0 1.0"/>
        </material>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
      </geometry>
    </collision>
    <!--First Link Small Box - Front -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.05 0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
        <material name="white">
          <color rgba="1.0 1.0 1.0 1.0"/>
        </material>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.05 0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
      </geometry>
    </collision>
    <!--First Link Box-->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.15 0"/>
      <geometry>
        <box size="0.05 0.2 0.05"/>
        <material name="white">
          <color rgba="1.0 1.0 1.0 1.0"/>
        </material>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.15 0"/>
      <geometry>
        <box size="0.05 0.2 0.05"/>
      </geometry>
    </collision>
    <!--First Link Small Box - Back -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.25 -0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
        <material name="white">
          <color rgba="1.0 1.0 1.0 1.0"/>
        </material>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.25 -0.0125"/>
      <geometry>
        <box size="0.05 0.1 0.025"/>
      </geometry>
    </collision>
    <!--First Link Small Cylinder - Back -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0.3 -0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
        <material name="white">
          <color rgba="1.0 1.0 1.0 1.0"/>
        </material>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.3 -0.0125"/>
      <geometry>
        <cylinder length="0.025" radius="0.025"/>
      </geometry>
    </collision>
  </link>

<joint name="anchor" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.55"/>
    <axis xyz="0 0 0"/>
  </joint>
  <joint name="first_joint" type="revolute">
    <parent link="base_link"/>
    <child link="first_link"/>
    <origin rpy="0 0 0.3925" xyz="0 0.075 0"/>
    <axis xyz="0 0 1"/>
    <calibration rising="1"/>
    <dynamic damping="0.0" friction="0.0"/>
    <limit effort="30" lower="-0.785" upper="0" velocity="1.0"/>
    <safety_controller k_position="15" k_velocity="10" soft_lower_limit="-1.5" soft_upper_limit="1.5"/>
  </joint>

</robot>
%MATLAB CODE
clear
close all
clc

addpath(genpath('/home/yeshi/codyco-superbuild'));

wbm_modelInitialiseFromURDF('/home/yeshi/gazebo-yeshi/1r_manipulator_scaled_down/model.urdf');

w_R_b = eye(3,3); % rotation matrix that transforms a vector in the base frame to the world frame
x_b = [0;0.0;0.0]; % position of the link frame origin wrt to the world frame
qj = [0.3925];  % joint positions
dqj = [0.0]; % joint velocities 
grav = [0;0;-9.8]; % gravity in world frame

dx_b = [0.0;0.0;0.0]; % derivative in the position of the link frame origin wrt to the world frame
omega_W = [0.0;0.0;0.0]; % angular velocity of base frame 

wbm_setWorldFrame(w_R_b,x_b,grav);
wbm_updateState(qj,dqj,[dx_b;omega_W]);

M = wbm_massMatrix(w_R_b,x_b,qj);

h = wbm_generalisedBiasForces();

world_T_frameb = wbm_forwardKinematics('base_link')
world_T_frame1 = wbm_forwardKinematics('first_link')

[s,v,d]=svd(M)
EIG=eig(M)
symm = M-M
traversaro commented 8 years ago

Just to clarify: the urdf code is missing the initial <robot> tag, right?

yeshasvitirupachuri commented 8 years ago

Yes, sorry about that The original URDF has it but I missed while pasting in this issue, now I edited it with the tag Thank you

traversaro commented 3 years ago

Hi @Yeshasvitvs, if this is still a problem, feel free to open an issue directly in the iDynTree repo.