orocos_kdl
articulatedbodyinertia.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
23 
24 #include <Eigen/Core>
25 
26 namespace KDL{
27 
29  {
30  this->M=Eigen::Matrix3d::Identity()*rbi.m;
31  this->I=Eigen::Map<const Eigen::Matrix3d>(rbi.I.data);
32  this->H << 0,-rbi.h[2],rbi.h[1],
33  rbi.h[2],0,-rbi.h[0],
34  -rbi.h[1],rbi.h[0],0;
35  }
36 
38  {
39  *this = RigidBodyInertia(m,c,Ic);
40  }
41 
42  ArticulatedBodyInertia::ArticulatedBodyInertia(const Eigen::Matrix3d& M, const Eigen::Matrix3d& H, const Eigen::Matrix3d& I)
43  {
44  this->M=M;
45  this->I=I;
46  this->H=H;
47  }
48 
50  return ArticulatedBodyInertia(a*I.M,a*I.H,a*I.I);
51  }
52 
54  return ArticulatedBodyInertia(Ia.M+Ib.M,Ia.H+Ib.H,Ia.I+Ib.I);
55  }
56 
58  return ArticulatedBodyInertia(Ia)+Ib;
59  }
61  return ArticulatedBodyInertia(Ia.M-Ib.M,Ia.H-Ib.H,Ia.I-Ib.I);
62  }
63 
65  return ArticulatedBodyInertia(Ia)-Ib;
66  }
67 
69  Wrench result;
70  Eigen::Vector3d::Map(result.force.data)=I.M*Eigen::Vector3d::Map(t.vel.data)+I.H.transpose()*Eigen::Vector3d::Map(t.rot.data);
71  Eigen::Vector3d::Map(result.torque.data)=I.I*Eigen::Vector3d::Map(t.rot.data)+I.H*Eigen::Vector3d::Map(t.vel.data);
72  return result;
73  }
74 
76  Frame X=T.Inverse();
77  //mb=ma
78  //hb=R*(h-m*r)
79  //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
80  Eigen::Map<Eigen::Matrix3d> E(X.M.data);
81  Eigen::Matrix3d rcross;
82  rcross << 0,-X.p[2],X.p[1],
83  X.p[2],0,-X.p[0],
84  -X.p[1],X.p[0],0;
85 
86  Eigen::Matrix3d HrM=I.H-rcross*I.M;
87  return ArticulatedBodyInertia(E*I.M*E.transpose(),E*HrM*E.transpose(),E*(I.I-rcross*I.H.transpose()+HrM*rcross)*E.transpose());
88  }
89 
91  Eigen::Map<const Eigen::Matrix3d> E(M.data);
92  return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E);
93  }
94 
96  //mb=ma
97  //hb=R*(h-m*r)
98  //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
99  Eigen::Matrix3d rcross;
100  rcross << 0,-p[2],p[1],
101  p[2],0,-p[0],
102  -p[1],p[0],0;
103 
104  Eigen::Matrix3d HrM=this->H-rcross*this->M;
105  return ArticulatedBodyInertia(this->M,HrM,this->I-rcross*this->H.transpose()+HrM*rcross);
106  }
107 }//namespace
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Definition: articulatedbodyinertia.cpp:49
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::RigidBodyInertia::h
Vector h
Definition: rigidbodyinertia.hpp:101
KDL::RigidBodyInertia::m
double m
Definition: rigidbodyinertia.hpp:100
KDL::ArticulatedBodyInertia
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL::ArticulatedBodyInertia::ArticulatedBodyInertia
ArticulatedBodyInertia()
Definition: articulatedbodyinertia.hpp:46
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Wrench::torque
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:885
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::ArticulatedBodyInertia::H
Eigen::Matrix3d H
Definition: articulatedbodyinertia.hpp:90
KDL::operator-
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:60
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::RotationalInertia
Definition: rotationalinertia.hpp:34
KDL::ArticulatedBodyInertia::RefPoint
ArticulatedBodyInertia RefPoint(const Vector &p)
Definition: articulatedbodyinertia.cpp:95
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Rotation::data
double data[9]
Definition: frames.hpp:306
KDL::ArticulatedBodyInertia::M
Eigen::Matrix3d M
Definition: articulatedbodyinertia.hpp:89
KDL::RotationalInertia::data
double data[9]
Definition: rotationalinertia.hpp:65
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
articulatedbodyinertia.hpp
KDL::RigidBodyInertia::I
RotationalInertia I
Definition: rigidbodyinertia.hpp:102
KDL::ArticulatedBodyInertia::I
Eigen::Matrix3d I
Definition: articulatedbodyinertia.hpp:91
KDL::operator+
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:53
KDL::Wrench::force
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:884
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303