Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / kdl / joint.cpp
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 "joint.hpp"
23
24 namespace KDL {
25
26     Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
27                  const double& _inertia, const double& _damping, const double& _stiffness):
28         type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
29     {
30                 // for sphere and swing, offset is not used, assume no offset
31     }
32
33     Joint::Joint(const Joint& in):
34         type(in.type),scale(in.scale),offset(in.offset),
35         inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
36     {
37     }
38
39     Joint& Joint::operator=(const Joint& in)
40     {
41         type=in.type;
42         scale=in.scale;
43         offset=in.offset;
44         inertia=in.inertia;
45         damping=in.damping;
46         stiffness=in.stiffness;
47                 return *this;
48     }
49
50
51     Joint::~Joint()
52     {
53     }
54
55     Frame Joint::pose(const double& q)const
56     {
57
58         switch(type){
59         case RotX:
60             return Frame(Rotation::RotX(scale*q+offset));
61             break;
62         case RotY:
63             return  Frame(Rotation::RotY(scale*q+offset));
64             break;
65         case RotZ:
66             return  Frame(Rotation::RotZ(scale*q+offset));
67             break;
68         case TransX:
69             return  Frame(Vector(scale*q+offset,0.0,0.0));
70             break;
71         case TransY:
72             return Frame(Vector(0.0,scale*q+offset,0.0));
73             break;
74         case TransZ:
75             return Frame(Vector(0.0,0.0,scale*q+offset));
76             break;
77                 case Sphere:
78                         // the joint angles represent a rotation vector expressed in the base frame of the joint
79                         // (= the frame you get when there is no offset nor rotation)
80                         return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2])));
81                         break;
82                 case Swing:
83                         // the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
84                         // (= the frame you get when there is no offset nor rotation)
85                         return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1])));
86                         break;
87         default:
88             return Frame::Identity();
89             break;
90         }
91     }
92
93     Twist Joint::twist(const double& qdot, int dof)const
94     {
95         switch(type){
96         case RotX:
97             return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
98             break;
99         case RotY:
100             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
101             break;
102         case RotZ:
103             return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
104             break;
105         case TransX:
106             return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
107             break;
108         case TransY:
109             return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
110             break;
111         case TransZ:
112             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
113             break;
114                 case Swing:
115                         switch (dof) {
116                         case 0:
117                                 return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
118                         case 1:
119                                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
120                         }
121             return Twist::Zero();
122                 case Sphere:
123                         switch (dof) {
124                         case 0:
125                                 return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
126                         case 1:
127                                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
128                         case 2:
129                                 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
130                         }
131             return Twist::Zero();
132         default:
133             return Twist::Zero();
134             break;
135         }
136         }
137
138         unsigned int Joint::getNDof() const
139         {
140                 switch (type) {
141                 case Sphere:
142                         return 3;
143                 case Swing:
144                         return 2;
145                 case None:
146                         return 0;
147                 default:
148                         return 1;
149                 }
150         }
151
152 } // end of namespace KDL
153