Blender  V3.3
joint.cpp
Go to the documentation of this file.
1 
4 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 
6 // Version: 1.0
7 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
8 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
9 // URL: http://www.orocos.org/kdl
10 
11 // This library is free software; you can redistribute it and/or
12 // modify it under the terms of the GNU Lesser General Public
13 // License as published by the Free Software Foundation; either
14 // version 2.1 of the License, or (at your option) any later version.
15 
16 // This library is distributed in the hope that it will be useful,
17 // but WITHOUT ANY WARRANTY; without even the implied warranty of
18 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 // Lesser General Public License for more details.
20 
21 // You should have received a copy of the GNU Lesser General Public
22 // License along with this library; if not, write to the Free Software
23 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 
25 #include "joint.hpp"
26 
27 namespace KDL {
28 
29  Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
30  const double& _inertia, const double& _damping, const double& _stiffness):
31  type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
32  {
33  // for sphere and swing, offset is not used, assume no offset
34  }
35 
36  Joint::Joint(const Joint& in):
37  type(in.type),scale(in.scale),offset(in.offset),
38  inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
39  {
40  }
41 
43  {
44  type=in.type;
45  scale=in.scale;
46  offset=in.offset;
47  inertia=in.inertia;
48  damping=in.damping;
49  stiffness=in.stiffness;
50  return *this;
51  }
52 
53 
55  {
56  }
57 
58  Frame Joint::pose(const double* q)const
59  {
60 
61  switch(type){
62  case RotX:
63  assert(q);
64  return Frame(Rotation::RotX(scale*q[0]+offset));
65  break;
66  case RotY:
67  assert(q);
68  return Frame(Rotation::RotY(scale*q[0]+offset));
69  break;
70  case RotZ:
71  assert(q);
72  return Frame(Rotation::RotZ(scale*q[0]+offset));
73  break;
74  case TransX:
75  assert(q);
76  return Frame(Vector(scale*q[0]+offset,0.0,0.0));
77  break;
78  case TransY:
79  assert(q);
80  return Frame(Vector(0.0,scale*q[0]+offset,0.0));
81  break;
82  case TransZ:
83  assert(q);
84  return Frame(Vector(0.0,0.0,scale*q[0]+offset));
85  break;
86  case Sphere:
87  // the joint angles represent a rotation vector expressed in the base frame of the joint
88  // (= the frame you get when there is no offset nor rotation)
89  assert(q);
90  return Frame(Rot(Vector(q[0], q[1], q[2])));
91  break;
92  case Swing:
93  // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
94  // (= the frame you get when there is no offset nor rotation)
95  assert(q);
96  return Frame(Rot(Vector(q[0], 0.0, q[1])));
97  break;
98  default:
99  return Frame::Identity();
100  break;
101  }
102  }
103 
104  Twist Joint::twist(const double& qdot, int dof)const
105  {
106  switch(type){
107  case RotX:
108  return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
109  break;
110  case RotY:
111  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
112  break;
113  case RotZ:
114  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
115  break;
116  case TransX:
117  return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
118  break;
119  case TransY:
120  return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
121  break;
122  case TransZ:
123  return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
124  break;
125  case Swing:
126  switch (dof) {
127  case 0:
128  return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
129  case 1:
130  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
131  }
132  return Twist::Zero();
133  case Sphere:
134  switch (dof) {
135  case 0:
136  return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
137  case 1:
138  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
139  case 2:
140  return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
141  }
142  return Twist::Zero();
143  default:
144  return Twist::Zero();
145  break;
146  }
147  }
148 
149  unsigned int Joint::getNDof() const
150  {
151  switch (type) {
152  case Sphere:
153  return 3;
154  case Swing:
155  return 2;
156  case None:
157  return 0;
158  default:
159  return 1;
160  }
161  }
162 
163 } // end of namespace KDL
164 
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum type
struct Frame Frame
Definition: frames.hpp:261
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
static Frame Identity()
Definition: frames.inl:719
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:43
Joint(const JointType &type=None, const double &scale=1, const double &offset=0, const double &inertia=0, const double &damping=0, const double &stiffness=0)
Definition: joint.cpp:29
Twist twist(const double &qdot, int dof=0) const
Definition: joint.cpp:104
@ Sphere
Definition: joint.hpp:45
@ TransZ
Definition: joint.hpp:45
@ TransY
Definition: joint.hpp:45
@ TransX
Definition: joint.hpp:45
virtual ~Joint()
Definition: joint.cpp:54
Joint & operator=(const Joint &arg)
Definition: joint.cpp:42
unsigned int getNDof() const
Definition: joint.cpp:149
Frame pose(const double *q) const
Definition: joint.cpp:58
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:609
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:614
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:619
represents both translational and rotational velocities.
Definition: frames.hpp:679
static Twist Zero()
Definition: frames.inl:310
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:143
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.inl:1143
ccl_gpu_kernel_postfix ccl_global float int int int int float bool int offset
Definition: chain.cpp:27