1 #ifndef ROBOTICS_KINEMATIC_CHAIN_H 2 #define ROBOTICS_KINEMATIC_CHAIN_H 7 #include <KrisLibrary/math/vector.h> 8 #include <KrisLibrary/math/matrix.h> 14 enum Type { TypeNone, TypeRotation, TypeTranslation, TypeCombination };
17 void SetRotationJoint(Real w);
18 void SetTranslationJoint(
const Vector2& v);
24 void GetLocalTransform(Real qi,
Frame2D& T)
const {
29 void GetVelocity(Real qi,Real dqi,
const Vector2& p,
Vector2& vel)
const;
30 void GetAngularVelocity(Real qi,Real dqi,Real& omega)
const;
33 void GetJacobian(Real qi,
const Vector2& p,Real& Jo,
Vector2& Jp)
const;
34 void GetOrientationJacobian(
Vector2& Jo)
const;
35 void GetPositionJacobian(Real qi,
const Vector2& p,
Vector2& Jp)
const;
38 void GetJacobian(Real qi,
Frame2D& J)
const;
40 void GetJacobian(Real qi,
const Frame2D& Tj_World,
Frame2D& J)
const;
53 void Initialize(
int numLinks)
55 links.resize(numLinks);
56 parents.resize(numLinks);
57 q.resize(numLinks,Zero);
58 qMin.resize(numLinks,-Inf);
59 qMax.resize(numLinks,Inf);
62 virtual std::string LinkName(
int i)
const 65 sprintf(temp,
"Link[%d]",i);
69 inline bool InJointLimits(
const Vector& q)
const 71 assert(q.n == qMin.n);
72 for(
int i=0;i<qMin.n;i++) {
73 if(qMin[i]>q[i] || q[i]>qMax[i]) {
86 void GetWorldRotation(
int i, Real& theta)
const;
88 void GetWorldVelocity(
const Vector2& pi,
int i,
const Vector& dq,
Vector2& dp)
const;
90 void GetWorldAngularVelocity(
int i,
const Vector& dq, Real& dtheta)
const;
93 bool GetJacobian(
const Vector2& pi,
int i,
int j, Real& dw,
Vector2& dv)
const;
94 bool GetOrientationJacobian(
int i,
int j, Real& dw)
const;
95 bool GetPositionJacobian(
const Vector2& pi,
int i,
int j,
Vector2& dv)
const;
97 void GetFullJacobian(
const Vector2& pi,
int i, Matrix& J)
const;
99 void GetPositionJacobian(
const Vector2& pi,
int i, Matrix& J)
const;
101 std::vector<Link> links;
108 template <
class Link>
112 assert(this->HasValidOrdering());
119 for(
unsigned int i=0;i<this->parents.size();i++) {
120 links[i].GetLocalTransform(q(i),Ti);
121 if(this->parents[i]==-1)
122 links[i].T_World=links[i].T0_Parent*Ti;
124 links[i].T_World=links[this->parents[i]].T_World*links[i].T0_Parent*Ti;
128 template <
class Link>
131 links[i].T_World.mulPoint(pi,p);
134 template <
class Link>
138 links[i].T_World.get(theta,tmp);
141 template <
class Link>
147 links[i].T_World.mul(pi,p);
150 links[j].GetVelocity(q[j],dq[j],p,temp);
156 template <
class Link>
163 links[j].GetAngularVelocity(q[j],dq[j],temp);
169 template <
class Link>
172 if(this->IsAncestor(i,j)) {
174 GetWorldPosition(pi,i,p);
175 links[j].GetJacobian(q[j],p,dw,dv);
183 template <
class Link>
186 if(IsAncestor(i,j)) {
187 links[j].GetOrientationJacobian(dw);
194 template <
class Link>
197 if(IsAncestor(i,j)) {
199 GetWorldPosition(pi,i,p);
200 links[j].GetPositionJacobian(q[j],p,dv);
207 template <
class Link>
210 J.resize(3,q.n,Zero);
213 GetWorldPosition(pi,i,p);
216 links[j].GetJacobian(q[j],p,w,v);
224 template <
class Link>
227 J.resize(3,q.n,Zero);
230 GetWorldPosition(pi,i,p);
233 links[j].GetPositionJacobian(q[j],p,v);
Definition: KinematicChain.h:48
The logging system used in KrisLibrary.
A tree-based chain structure.
Definition: Chain.h:16
A 2D vector class.
Definition: math3d/primitives.h:41
Definition: KinematicChain.h:11