KrisLibrary  1.0.0
KinematicChain.h
1 #ifndef ROBOTICS_KINEMATIC_CHAIN_H
2 #define ROBOTICS_KINEMATIC_CHAIN_H
3 
4 #include <KrisLibrary/Logger.h>
5 #include "Frame.h"
6 #include "Chain.h"
7 #include <KrisLibrary/math/vector.h>
8 #include <KrisLibrary/math/matrix.h>
9 #include <string>
10 
12 {
13 public:
14  enum Type { TypeNone, TypeRotation, TypeTranslation, TypeCombination };
15 
16  Type GetType() const;
17  void SetRotationJoint(Real w);
18  void SetTranslationJoint(const Vector2& v);
19 
20  //screw's transformation is a function of control qi
21  //say we're link i
22  //T(i->i)(qi) = R(w*qi)oT(v*qi) where R rotates the given angle, T translates
23  //T(i->pi)(qi) = T0(i->pi)*T(i->i)(qi)
24  void GetLocalTransform(Real qi,Frame2D& T) const {
25  T.set(qi*w,qi*v);
26  }
27 
28  //evaluations of a point (in frame 0) with respect to qi,dqi
29  void GetVelocity(Real qi,Real dqi,const Vector2& p,Vector2& vel) const;
30  void GetAngularVelocity(Real qi,Real dqi,Real& omega) const;
31 
32  //Jacobian (orientation,position) of a point (in frame 0) w.r.t. i
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;
36 
37  //Position jacobian of points in frame 0 w.r.t. i
38  void GetJacobian(Real qi,Frame2D& J) const;
39  //Position jacobian of points in frame j w.r.t. i
40  void GetJacobian(Real qi,const Frame2D& Tj_World,Frame2D& J) const;
41 
42  Real w; Vector2 v;
43  Frame2D T0_Parent;
44  Frame2D T_World;
45 };
46 
47 template <class Link>
49 {
50 public:
51  virtual ~KinematicChain2DTemplate() {}
52 
53  void Initialize(int numLinks)
54  {
55  links.resize(numLinks);
56  parents.resize(numLinks);
57  q.resize(numLinks,Zero);
58  qMin.resize(numLinks,-Inf);
59  qMax.resize(numLinks,Inf);
60  }
61 
62  virtual std::string LinkName(int i) const
63  {
64  char temp[20];
65  sprintf(temp,"Link[%d]",i);
66  return temp;
67  }
68 
69  inline bool InJointLimits(const Vector& q) const
70  {
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]) {
74  return false;
75  }
76  }
77  return true;
78  }
79 
80  //based on the values in q, update the frames T
81  void UpdateFrames();
82  //pi is a point in the local frame of body i
83  //gets the world location of pi
84  void GetWorldPosition(const Vector2& pi, int i, Vector2& p) const;
85  //gets the world rotation of link i
86  void GetWorldRotation(int i, Real& theta) const;
87  //gets the world velocity of pi, given dq/dt
88  void GetWorldVelocity(const Vector2& pi, int i, const Vector& dq, Vector2& dp) const;
89  //gets the world angular velocity of frame of body i
90  void GetWorldAngularVelocity(int i, const Vector& dq, Real& dtheta) const;
91 
92  //gets the jacobian of pi w.r.t qj
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;
96  //gets the jacobian of pi w.r.t q (row 0 is angular, 1,2 are translational)
97  void GetFullJacobian(const Vector2& pi, int i, Matrix& J) const;
98  //rows 1,2 of the above
99  void GetPositionJacobian(const Vector2& pi, int i, Matrix& J) const;
100 
101  std::vector<Link> links;
102  Vector q;
103  Vector qMin,qMax;
104 };
105 
107 
108 template <class Link>
110 {
111  //based on the values in q, update the frames T
112  assert(this->HasValidOrdering());
113  Frame2D Ti;
114  //get local transform T(i->i)(q)
115  //then do the parent transformations to get them to T(i->0)(q)
116  //since the chain is top down, we can loop straight through
117  //given parent j and T(j->0)(q), we have
118  //T(i->0)(q) = T(j->0)(q)*T0(i->j)*T(i->i)(q)
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;
123  else
124  links[i].T_World=links[this->parents[i]].T_World*links[i].T0_Parent*Ti;
125  }
126 }
127 
128 template <class Link>
130 {
131  links[i].T_World.mulPoint(pi,p);
132 }
133 
134 template <class Link>
135 void KinematicChain2DTemplate<Link>::GetWorldRotation(int i, Real& theta) const
136 {
137  Vector2 tmp;
138  links[i].T_World.get(theta,tmp);
139 }
140 
141 template <class Link>
142 void KinematicChain2DTemplate<Link>::GetWorldVelocity(const Vector2& pi, int i, const Vector& dq, Vector2& dp) const
143 {
144  Vector2 temp;
145  dp.setZero();
146  Vector2 p;
147  links[i].T_World.mul(pi,p);
148  int j=i;
149  while(j!=-1) {
150  links[j].GetVelocity(q[j],dq[j],p,temp);
151  dp+=temp;
152  j=this->parents[j];
153  }
154 }
155 
156 template <class Link>
157 void KinematicChain2DTemplate<Link>::GetWorldAngularVelocity(int i, const Vector& dq, Real& dtheta) const
158 {
159  Real temp;
160  dtheta = Zero;
161  int j=i;
162  while(j!=-1) {
163  links[j].GetAngularVelocity(q[j],dq[j],temp);
164  dtheta+=temp;
165  j=this->parents[j];
166  }
167 }
168 
169 template <class Link>
170 bool KinematicChain2DTemplate<Link>::GetJacobian(const Vector2& pi, int i, int j, Real& dw, Vector2& dv) const
171 {
172  if(this->IsAncestor(i,j)) {
173  Vector2 p;
174  GetWorldPosition(pi,i,p);
175  links[j].GetJacobian(q[j],p,dw,dv);
176  return true;
177  }
178  dw=Zero;
179  dv.setZero();
180  return false;
181 }
182 
183 template <class Link>
184 bool KinematicChain2DTemplate<Link>::GetOrientationJacobian(int i, int j, Real& dw) const
185 {
186  if(IsAncestor(i,j)) {
187  links[j].GetOrientationJacobian(dw);
188  return true;
189  }
190  dw=Zero;
191  return false;
192 }
193 
194 template <class Link>
195 bool KinematicChain2DTemplate<Link>::GetPositionJacobian(const Vector2& pi, int i, int j, Vector2& dv) const
196 {
197  if(IsAncestor(i,j)) {
198  Vector2 p;
199  GetWorldPosition(pi,i,p);
200  links[j].GetPositionJacobian(q[j],p,dv);
201  return true;
202  }
203  dv.setZero();
204  return false;
205 }
206 
207 template <class Link>
208 void KinematicChain2DTemplate<Link>::GetFullJacobian(const Vector2& pi, int i, Matrix& J) const
209 {
210  J.resize(3,q.n,Zero);
211  Real w; Vector2 v;
212  Vector2 p;
213  GetWorldPosition(pi,i,p);
214  int j=i;
215  while(j!=-1) {
216  links[j].GetJacobian(q[j],p,w,v);
217  J(0,j)=w;
218  J(1,j)=v.x;
219  J(2,j)=v.y;
220  j=parents[j];
221  }
222 }
223 
224 template <class Link>
225 void KinematicChain2DTemplate<Link>::GetPositionJacobian(const Vector2& pi, int i, Matrix& J) const
226 {
227  J.resize(3,q.n,Zero);
228  Vector2 v;
229  Vector2 p;
230  GetWorldPosition(pi,i,p);
231  int j=i;
232  while(j!=-1) {
233  links[j].GetPositionJacobian(q[j],p,v);
234  J(0,j)=v.x;
235  J(1,j)=v.y;
236  j=parents[j];
237  }
238 }
239 
240 
241 #endif
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
Same as above, but in 2D.
Definition: math3d/primitives.h:902
Definition: KinematicChain.h:11