00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "joint.hpp"
00023
00024 namespace KDL {
00025
00026
00027 Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset,
00028 const double& _inertia, const double& _damping, const double& _stiffness):
00029 name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00030 {
00031 if (type == RotAxis || type == TransAxis) throw joint_type_ex;
00032 q_previous = 0;
00033 }
00034
00035
00036 Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
00037 const double& _inertia, const double& _damping, const double& _stiffness):
00038 name("NoName"),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00039 {
00040 if (type == RotAxis || type == TransAxis) throw joint_type_ex;
00041 q_previous = 0;
00042 }
00043
00044
00045 Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
00046 const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00047 name(_name), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
00048 , axis(_axis / _axis.Norm()), origin(_origin)
00049 {
00050 if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00051
00052
00053 joint_pose.p = origin;
00054 joint_pose.M = Rotation::Rot2(axis, offset);
00055 q_previous = 0;
00056 }
00057
00058
00059 Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
00060 const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
00061 name("NoName"), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness),
00062 axis(_axis / _axis.Norm()),origin(_origin)
00063 {
00064 if (type != RotAxis && type != TransAxis) throw joint_type_ex;
00065
00066
00067 joint_pose.p = origin;
00068 joint_pose.M = Rotation::Rot2(axis, offset);
00069 q_previous = 0;
00070 }
00071
00072 Joint::~Joint()
00073 {
00074 }
00075
00076 Frame Joint::pose(const double& q)const
00077 {
00078 switch(type){
00079 case RotAxis:
00080
00081 if (q != q_previous){
00082 q_previous = q;
00083 joint_pose.M = Rotation::Rot2(axis, scale*q+offset);
00084 }
00085 return joint_pose;
00086 case RotX:
00087 return Frame(Rotation::RotX(scale*q+offset));
00088 case RotY:
00089 return Frame(Rotation::RotY(scale*q+offset));
00090 case RotZ:
00091 return Frame(Rotation::RotZ(scale*q+offset));
00092 case TransAxis:
00093 return Frame(origin + (axis * (scale*q+offset)));
00094 case TransX:
00095 return Frame(Vector(scale*q+offset,0.0,0.0));
00096 case TransY:
00097 return Frame(Vector(0.0,scale*q+offset,0.0));
00098 case TransZ:
00099 return Frame(Vector(0.0,0.0,scale*q+offset));
00100 case None:
00101 return Frame::Identity();
00102 }
00103 return Frame::Identity();
00104 }
00105
00106 Twist Joint::twist(const double& qdot)const
00107 {
00108 switch(type){
00109 case RotAxis:
00110 return Twist(Vector(0,0,0), axis * ( scale * qdot));
00111 case RotX:
00112 return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
00113 case RotY:
00114 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,scale*qdot,0.0));
00115 case RotZ:
00116 return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
00117 case TransAxis:
00118 return Twist(axis * (scale * qdot), Vector(0,0,0));
00119 case TransX:
00120 return Twist(Vector(scale*qdot,0.0,0.0),Vector(0.0,0.0,0.0));
00121 case TransY:
00122 return Twist(Vector(0.0,scale*qdot,0.0),Vector(0.0,0.0,0.0));
00123 case TransZ:
00124 return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
00125 case None:
00126 return Twist::Zero();
00127 }
00128 return Twist::Zero();
00129 }
00130
00131 Vector Joint::JointAxis() const
00132 {
00133 switch(type)
00134 {
00135 case RotAxis:
00136 return axis;
00137 case RotX:
00138 return Vector(1.,0.,0.);
00139 case RotY:
00140 return Vector(0.,1.,0.);
00141 case RotZ:
00142 return Vector(0.,0.,1.);
00143 case TransAxis:
00144 return axis;
00145 case TransX:
00146 return Vector(1.,0.,0.);
00147 case TransY:
00148 return Vector(0.,1.,0.);
00149 case TransZ:
00150 return Vector(0.,0.,1.);
00151 case None:
00152 return Vector::Zero();
00153 }
00154 return Vector::Zero();
00155 }
00156
00157 Vector Joint::JointOrigin() const
00158 {
00159 return origin;
00160 }
00161
00162 }
00163