orocos_kdl
src
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
22
#include "
articulatedbodyinertia.hpp
"
23
24
#include <Eigen/Core>
25
26
namespace
KDL
{
27
28
ArticulatedBodyInertia::ArticulatedBodyInertia
(
const
RigidBodyInertia
& rbi)
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
37
ArticulatedBodyInertia::ArticulatedBodyInertia
(
double
m,
const
Vector
& c,
const
RotationalInertia
& Ic)
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
49
ArticulatedBodyInertia
operator*
(
double
a,
const
ArticulatedBodyInertia
& I){
50
return
ArticulatedBodyInertia
(a*I.
M
,a*I.
H
,a*I.
I
);
51
}
52
53
ArticulatedBodyInertia
operator+
(
const
ArticulatedBodyInertia
& Ia,
const
ArticulatedBodyInertia
& Ib){
54
return
ArticulatedBodyInertia
(Ia.
M
+Ib.
M
,Ia.
H
+Ib.
H
,Ia.
I
+Ib.
I
);
55
}
56
57
ArticulatedBodyInertia
operator+
(
const
RigidBodyInertia
& Ia,
const
ArticulatedBodyInertia
& Ib){
58
return
ArticulatedBodyInertia
(Ia)+Ib;
59
}
60
ArticulatedBodyInertia
operator-
(
const
ArticulatedBodyInertia
& Ia,
const
ArticulatedBodyInertia
& Ib){
61
return
ArticulatedBodyInertia
(Ia.
M
-Ib.
M
,Ia.
H
-Ib.
H
,Ia.
I
-Ib.
I
);
62
}
63
64
ArticulatedBodyInertia
operator-
(
const
RigidBodyInertia
& Ia,
const
ArticulatedBodyInertia
& Ib){
65
return
ArticulatedBodyInertia
(Ia)-Ib;
66
}
67
68
Wrench
operator*
(
const
ArticulatedBodyInertia
& I,
const
Twist
& t){
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
75
ArticulatedBodyInertia
operator*
(
const
Frame
& T,
const
ArticulatedBodyInertia
& I){
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
90
ArticulatedBodyInertia
operator*
(
const
Rotation
& M,
const
ArticulatedBodyInertia
& I){
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
95
ArticulatedBodyInertia
ArticulatedBodyInertia::RefPoint
(
const
Vector
& p){
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
Generated on Thu Nov 14 2024 04:37:48 for orocos_kdl by
1.8.17