Problem with using Centre of Mass on Matlab robotics toolbox
2 次查看(过去 30 天)
显示 更早的评论
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 个)
Gabriele Gualandi
2022-3-5
编辑: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).
0 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Robotics 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!