Problem with using Centre of Mass on Matlab robotics toolbox

Hi,

I am using MATLAB robotics toolbox for a simple two link manipulator ( two revolute joints ). For simplicity, both links are of length 1m and weigh 1kg.

When using the centreOfMass function, I got some weird results. For example, if I put the joint position at 0 0 (for the two angles ), the centre of mass should obviously be at (1,0), but instead I get (2,0). Using formulae, I get the correct result.

It may be that I have made some mistake in defining manipulator via DH parameters but I just can't find it. Visualization looks fine. Joint torques have a problem as well.

    %Two link simulator
    clear all; close all; clc;

    l1 = 1;
    l2 = 1;

    row = 1;

    m1 = row*l1;
    m2 = row*l2;

    I1 = m1*(l1^2)/12;
    I2 = m2*(l2^2)/12;

    g = 9.81;

    theta1 = 0*pi/180;
    theta2 = 0*pi/180;
    theta1_dot_dot = 0.2;
    theta2_dot_dot = 0.2;

    %Define the robot

    dhparams = [  l1  0    0    0;
                  l2  0    0    0 ];

    twoLinkArmRobot = robotics.RigidBodyTree;

    body1 = robotics.RigidBody('body1');
    body1.Mass = m1;
    body1.CenterOfMass = [ l1/2 0 0 ];
    body1.Inertia = [ I1 I1 I1 0 0 0 ];
    jnt1 = robotics.Joint('jnt1','revolute');
    setFixedTransform(jnt1,dhparams(1,:),'dh');
    body1.Joint = jnt1;

    body2 = robotics.RigidBody('body2');
    body2.Mass = m2;
    body2.CenterOfMass = [ l2/2 0 0 ];
    body2.Inertia = [ I2 I2 I2 0 0 0 ];
    jnt2 = robotics.Joint('jnt2','revolute');
    setFixedTransform(jnt2,dhparams(2,:),'dh');
    body2.Joint = jnt2;

    addBody(twoLinkArmRobot,body1,'base')
    addBody(twoLinkArmRobot,body2,'body1')

    showdetails(twoLinkArmRobot)

    twoLinkArmRobot.DataFormat = 'row';
    twoLinkArmRobot.Gravity = [0 -g 0];

    config = [ theta1 -theta1+theta2 ];

    figure
    show(twoLinkArmRobot,config);

    centre_of_mass = centerOfMass(twoLinkArmRobot,config)
    gravity_torque = gravityTorque(twoLinkArmRobot,config)
    joint_torques = inverseDynamics(twoLinkArmRobot,config,[ 0 0 ],[ theta1_dot_dot theta2_dot_dot])

    %According to calculations
    cm = [ m1*l1*cos(theta1)/2+m2*(l1*cos(theta1)+l2*cos(theta2)/2)  m1*l1*sin(theta1)/2+m2*(l1*sin(theta1)+l2*sin(theta2)/2) ]/(m1+m2) 
    tau2 = I2*theta2_dot_dot + m2*g*l2*cos(theta2)/2
    tau1 = I1*theta1_dot_dot + m1*g*l1*cos(theta1)/2 + m2*g*l1*cos(theta1)

[snow John]

Hello sir，

did you solve this problem？

Answer 1

[Gabriele Gualandi]

Considering i-th link, i = 1...n, you should express the CenterOfMass vector in the i-th reference frame, which is the one rigidly attached to link i. Instead, you seem to express that vector in the (i-1)-th frame. So you can fix your code by setting:

body1.CenterOfMass = [ -l1/2 0 0 ];

body2.CenterOfMass = [ -l2/2 0 0 ];

Note: express the COM position in the (i-1)-th frame is not convenient becase when you activate i-th joint such vector would change (i.e., it's not constant).