orocos_kdl
joint.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
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 "joint.hpp"
23 
24 namespace KDL {
25 
26  // constructor for joint along x,y or z axis, at origin of reference frame
27  Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset,
28  const double& _inertia, const double& _damping, const double& _stiffness):
29  name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
30  {
31  if (type == RotAxis || type == TransAxis)
32  throw joint_type_ex;
33  }
34 
35  // constructor for joint along x,y or z axis, at origin of reference frame
36  Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
37  const double& _inertia, const double& _damping, const double& _stiffness):
38  name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
39  {
40  if (type == RotAxis || type == TransAxis)
41  throw joint_type_ex;
42  }
43 
44  // constructor for joint along arbitrary axis, at arbitrary origin
45  Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
46  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
47  name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
48  , axis(_axis / _axis.Norm()), origin(_origin)
49  {
50  if (type != RotAxis && type != TransAxis)
51  throw joint_type_ex;
52  }
53 
54  // constructor for joint along arbitrary axis, at arbitrary origin
55  Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
56  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
57  name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
58  axis(_axis / _axis.Norm()),origin(_origin)
59  {
60  if (type != RotAxis && type != TransAxis)
61  throw joint_type_ex;
62  }
63 
65  {
66  }
67 
68  Frame Joint::pose(const double& q)const
69  {
70  switch(type){
71  case RotAxis:
73  case RotX:
74  return Frame(Rotation::RotX(scale*q+offset));
75  case RotY:
76  return Frame(Rotation::RotY(scale*q+offset));
77  case RotZ:
78  return Frame(Rotation::RotZ(scale*q+offset));
79  case TransAxis:
80  return Frame(origin + (axis * (scale*q+offset)));
81  case TransX:
82  return Frame(Vector(scale*q+offset,0.0,0.0));
83  case TransY:
84  return Frame(Vector(0.0,scale*q+offset,0.0));
85  case TransZ:
86  return Frame(Vector(0.0,0.0,scale*q+offset));
87  case Fixed:
88  return Frame::Identity();
89  }
90  return Frame::Identity();
91  }
92 
93  Twist Joint::twist(const double& qdot)const
94  {
95  switch(type){
96  case RotAxis:
97  return Twist(Vector(0,0,0), axis * ( scale * qdot));
98  case RotX:
99  return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
100  case RotY:
101  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
102  case RotZ:
103  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
104  case TransAxis:
105  return Twist(axis * (scale * qdot), Vector(0,0,0));
106  case TransX:
107  return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
108  case TransY:
109  return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
110  case TransZ:
111  return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
112  case Fixed:
113  return Twist::Zero();
114  }
115  return Twist::Zero();
116  }
117 
119  {
120  switch(type)
121  {
122  case RotAxis:
123  return axis;
124  case RotX:
125  return Vector(1.,0.,0.);
126  case RotY:
127  return Vector(0.,1.,0.);
128  case RotZ:
129  return Vector(0.,0.,1.);
130  case TransAxis:
131  return axis;
132  case TransX:
133  return Vector(1.,0.,0.);
134  case TransY:
135  return Vector(0.,1.,0.);
136  case TransZ:
137  return Vector(0.,0.,1.);
138  case Fixed:
139  return Vector::Zero();
140  }
141  return Vector::Zero();
142  }
143 
145  {
146  return origin;
147  }
148 
149 } // end of namespace KDL
150 
KDL::Rotation::Rot2
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:328
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::Joint::RotZ
@ RotZ
Definition: joint.hpp:47
std::string
KDL::Joint::RotY
@ RotY
Definition: joint.hpp:47
KDL::Joint::joint_type_ex
KDL::Joint::joint_type_exception joint_type_ex
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Joint::TransZ
@ TransZ
Definition: joint.hpp:47
KDL::Joint::twist
Twist twist(const double &qdot) const
Definition: joint.cpp:93
KDL::Joint::Joint
Joint(const std::string &name, const JointType &type=Fixed, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
Definition: joint.cpp:27
KDL::Joint::JointOrigin
Vector JointOrigin() const
Definition: joint.cpp:144
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
KDL::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Joint::pose
Frame pose(const double &q) const
Definition: joint.cpp:68
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::Joint::TransAxis
@ TransAxis
Definition: joint.hpp:47
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Joint::scale
double scale
Definition: joint.hpp:270
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:440
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
KDL::Joint::RotX
@ RotX
Definition: joint.hpp:47
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::Joint::~Joint
virtual ~Joint()
Definition: joint.cpp:64
KDL::Joint::TransX
@ TransX
Definition: joint.hpp:47
KDL::Joint::RotAxis
@ RotAxis
Definition: joint.hpp:47
KDL::Twist::Zero
static Twist Zero()
Definition: frames.inl:290
KDL::Joint::offset
double offset
Definition: joint.hpp:271
KDL::Joint::JointType
JointType
Definition: joint.hpp:47
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
KDL::Joint::TransY
@ TransY
Definition: joint.hpp:47
KDL::Joint::origin
Vector origin
Definition: joint.hpp:277
KDL::Joint::axis
Vector axis
Definition: joint.hpp:277
joint.hpp
KDL::Joint::type
Joint::JointType type
Definition: joint.hpp:269
KDL::Joint::JointAxis
Vector JointAxis() const
Definition: joint.cpp:118