Why the robot.Bodies.Inertia is different from URDF?
39 次查看(过去 30 天)
显示 更早的评论
Hello guys,I have used the importrobot('robot.urdf') generate the rigidbodytree,But the Inertia in rigidbodytree are not the same with URDF file
Can someone explain this?
Thanks in advance.The pictures show below for better understanding the problem.
By the way ,Do you use the Robotics System Toolbox to calculate robot dynamics and compare them with Peter Robotics Tools? Is there a big difference between the results of the two calculations or is there anything that needs attention?
Best regards,
-Jian
0 个评论
采纳的回答
Hannes Daepp
2021-12-6
编辑:Hannes Daepp
2021-12-6
This is an exepcted difference, and happens because URDF specifies the inertia with respect to the Center of Mass, while the RigidBody object defines inertia with respect to the frame origin (i.e. the pose of the body's joint frame).
While the inertia values themselves have different frames, they're functionally equivalent as long as the frame is accounted for in subsequent calculations. Further. any calls to the rigid body tree dynamics methods such as forwardDynamics, inverseDynamics, etc., should produce the same results as an equivalent call on the URDF. The dynamics functions shipped with Robotics System Toolbox are verified in test and held to our usual high quality standards. However, if you experience unexpected results or have specific concerns regarding the results of a dynamics computation, please contact technical support.
Regarding the specific property in question: this relationship is defined by a similarity transform for the rotation and (the tensor generalization of) the parallel axis theorem for the translation:
where is the inertia in the rigid body reference frame, is the inertia in the reference frame used by URDF, is the rotation from the URDF frame to the rigid body origin frame, m is the mass, is the translation vector between the two frames, and denotes a 3x3 identity matrix. Using skew symmetric identities, this relationship can be stated more concisely as
where sk(p) denotes the skew-symmetric matrix associated to the position vector .
For the numbers you've provided, it looks like the rotation element is likely zero () since these values can be derived from a parallel axis shift alone:
p = [.026887; 0.000909; 0.141285];
m = 5.31726;
ixx = .225449; ixy = -.000426; ixz = 0.001273; iyy = .233014; iyz = -.000046; izz = 0.023024;
I_urdf = [ixx, ixy, ixz; ixy, iyy, iyz; ixz iyz izz];
p_skew = [0, -p(3), p(2); p(3), 0, -p(1); -p(2), p(1), 0];
I_rb = eye(3)*I_urdf*eye(3)' - m*p_skew^2;
% The flattened form you are reading out is stored in the order
% [Ixx Iyy Izz Iyz Ixz Ixy]
I_rb_vector = [I_rb(1,1), I_rb(2,2), I_rb(3,3), I_rb(2,3), I_rb(1,3), I_rb(1,2)]
更多回答(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!