joint.cpp
Go to the documentation of this file.
1 // Copyright (C) 2020 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) throw joint_type_ex;
32  q_previous = 0;
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) throw joint_type_ex;
41  q_previous = 0;
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) throw joint_type_ex;
51 
52  // initialize
55  q_previous = 0;
56  }
57 
58  // constructor for joint along arbitrary axis, at arbitrary origin
59  Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
60  const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
61  name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
62  axis(_axis / _axis.Norm()),origin(_origin)
63  {
64  if (type != RotAxis && type != TransAxis) throw joint_type_ex;
65 
66  // initialize
69  q_previous = 0;
70  }
71 
73  {
74  }
75 
76  Frame Joint::pose(const double& q)const
77  {
78  switch(type){
79  case RotAxis:
80  // calculate the rotation matrix around the vector "axis"
81  if (q != q_previous){
82  q_previous = q;
84  }
85  return joint_pose;
86  case RotX:
87  return Frame(Rotation::RotX(scale*q+offset));
88  case RotY:
89  return Frame(Rotation::RotY(scale*q+offset));
90  case RotZ:
91  return Frame(Rotation::RotZ(scale*q+offset));
92  case TransAxis:
93  return Frame(origin + (axis * (scale*q+offset)));
94  case TransX:
95  return Frame(Vector(scale*q+offset,0.0,0.0));
96  case TransY:
97  return Frame(Vector(0.0,scale*q+offset,0.0));
98  case TransZ:
99  return Frame(Vector(0.0,0.0,scale*q+offset));
100  case Fixed:
101  return Frame::Identity();
102  }
103  return Frame::Identity();
104  }
105 
106  Twist Joint::twist(const double& qdot)const
107  {
108  switch(type){
109  case RotAxis:
110  return Twist(Vector(0,0,0), axis * ( scale * qdot));
111  case RotX:
112  return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
113  case RotY:
114  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
115  case RotZ:
116  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
117  case TransAxis:
118  return Twist(axis * (scale * qdot), Vector(0,0,0));
119  case TransX:
120  return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
121  case TransY:
122  return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
123  case TransZ:
124  return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
125  case Fixed:
126  return Twist::Zero();
127  }
128  return Twist::Zero();
129  }
130 
132  {
133  switch(type)
134  {
135  case RotAxis:
136  return axis;
137  case RotX:
138  return Vector(1.,0.,0.);
139  case RotY:
140  return Vector(0.,1.,0.);
141  case RotZ:
142  return Vector(0.,0.,1.);
143  case TransAxis:
144  return axis;
145  case TransX:
146  return Vector(1.,0.,0.);
147  case TransY:
148  return Vector(0.,1.,0.);
149  case TransZ:
150  return Vector(0.,0.,1.);
151  case Fixed:
152  return Vector::Zero();
153  }
154  return Vector::Zero();
155  }
156 
158  {
159  return origin;
160  }
161 
162 } // end of namespace KDL
163 
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
static Twist Zero()
Definition: frames.hpp:291
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:606
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:616
double q_previous
Definition: joint.hpp:239
Frame pose(const double &q) const
Definition: joint.cpp:76
double damping
Definition: joint.hpp:233
Vector origin
Definition: joint.hpp:237
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.hpp:611
double stiffness
Definition: joint.hpp:234
Vector JointOrigin() const
Definition: joint.cpp:157
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
Vector axis
Definition: joint.hpp:237
represents both translational and rotational velocities.
Definition: frames.hpp:720
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
KDL::Joint::joint_type_exception joint_type_ex
Vector p
origine of the Frame
Definition: frames.hpp:572
std::string name
Definition: joint.hpp:228
double inertia
Definition: joint.hpp:232
virtual ~Joint()
Definition: joint.cpp:72
Frame joint_pose
Definition: joint.hpp:238
static Frame Identity()
Definition: frames.hpp:701
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
Joint::JointType type
Definition: joint.hpp:229
Vector JointAxis() const
Definition: joint.cpp:131
double offset
Definition: joint.hpp:231
static Rotation Rot2(const Vector &rotvec, double angle)
Along an arbitrary axes. rotvec should be normalized.
Definition: frames.cpp:303
double scale
Definition: joint.hpp:230
static Vector Zero()
Definition: frames.hpp:139
Twist twist(const double &qdot) const
Definition: joint.cpp:106


orocos_kdl
Author(s):
autogenerated on Thu Sep 24 2020 03:05:38