Problem with using Centre of Mass on Matlab robotics toolbox

Question

Saurabh Chatterjee 2017-9-5

1
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/355261-problem-with-using-centre-of-mass-on-matlab-robotics-toolbox

编辑： Gabriele Gualandi 2022-3-5

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)

1 个评论
显示 -1更早的评论隐藏 -1更早的评论

snow John 2019-7-4

Hello sir，

did you solve this problem？

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Gabriele Gualandi 2022-3-5

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/355261-problem-with-using-centre-of-mass-on-matlab-robotics-toolbox#answer_910409

编辑：Gabriele Gualandi 2022-3-5

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).