KrisLibrary  1.0.0
DynamicChain3D.h
1 #ifndef ROBOTICS_DYNAMIC_CHAIN3D_H
2 #define ROBOTICS_DYNAMIC_CHAIN3D_H
3 
4 #include "KinematicChain3D.h"
5 #include <KrisLibrary/structs/array2d.h>
6 
8 {
9 public:
10  void GetWorldInertia(Matrix3& inertiaWorld) const;
11  void GetWorldCOM(Vector3& comWorld) const { T_World.mulPoint(com,comWorld); }
12 
13  //in local frame
14  Real mass;
15  Matrix3 inertia;
16  Vector3 com;
17 };
18 
19 class DynamicChain3D : public KinematicChain3DTemplate<DynamicLink3D>
20 {
21 public:
22  void Initialize(int numBodies);
23  void UpdateDynamics(); //this updates frames as well as jacobians and other temporary storage
24  //temp storage updates -- full Jacobian must be updated before using the computation methods
25  void Update_J();
26  void Update_dB_dq();
27 
28  //Really the i,jth component of Hessian,
29  //the name "JacobianDeriv" is stuck historically
30  bool GetJacobianDeriv(const Vector3& pm, int m, int i, int j, Vector3& ddtheta,Vector3& ddp) const;
31  //assumes j<=i<=m
32  void GetJacobianDeriv_Fast(const Vector3& pm, int m, int i, int j, Vector3& ddtheta,Vector3& ddp) const;
33  void GetHessian(const Vector3& pm, int m, Matrix* Htheta[3], Matrix* Hp[3]) const;
34  void GetDirectionalHessian(const Vector3& pm, int m, const Vector3& v, Matrix& Hpv) const;
35  bool GetJacobianDt(const Vector3& pi, int i, int j, Vector3&dtheta_dt,Vector3& dp_dt) const;
36  void GetWorldAcceleration(const Vector3& pi, int i, const Vector& ddq, Vector3& dw,Vector3& dv) const;
37  void GetResidualAcceleration(const Vector3& pi, int i, Vector3& dw,Vector3& dv) const;
38  Real GetTotalMass() const;
39  Vector3 GetCOM() const;
40  void GetCOMJacobian(Matrix& Jc) const;
41  void GetCOMHessian(Matrix& Hx,Matrix& Hy,Matrix& Hz) const;
42  Real GetKineticEnergy(int i) const;
43  Real GetKineticEnergy() const;
44  void GetKineticEnergyMatrix(Matrix& B) const; //gets the derivative of the jacobian dpi/dqj wrt qk, i.e. ddpi/dqjdqk
45  //dBij/dqz
46  Real GetKineticEnergyDeriv(int i,int j,int z) const;
47  void GetKineticEnergyMatrixDeriv(int z,Matrix& dB) const;
48  //dB/dt
49  void GetKineticEnergyMatrixTimeDeriv(Matrix& dB) const;
50  void GetCoriolisForceMatrix(Matrix& C);
51  void GetCoriolisForces(Vector& Cdq);
52  //torque/force pair on body i (in world coordinates)
53  void GetForceVector(const Vector3& torque, const Vector3& force, int i, Vector& F) const;
54  void AddForceVector(const Vector3& torque, const Vector3& force, int i, Vector& F) const;
55  void GetGravityVector(const Vector3& g0, Vector& G) const;
56  Real GetGravityPotentialEnergy(const Vector3& g0);
57 
58  //B*ddq + C*dq = fext
59  void GetAcceleration(Vector& ddq, const Vector& fext);
60 
61  Vector dq;
62 
63  //temp storage
66  std::vector<Matrix> dB_dq;
67 };
68 
69 
70 #endif
A 3D vector class.
Definition: math3d/primitives.h:136
Definition: KinematicChain3D.h:10
Definition: KinematicChain3D.h:45
A 3x3 matrix class.
Definition: math3d/primitives.h:469
Definition: DynamicChain3D.h:19
Definition: DynamicChain3D.h:7