KrisLibrary  1.0.0
RobotDynamics3D.h
1 #ifndef ROBOT_DYNAMICS_3D_H
2 #define ROBOT_DYNAMICS_3D_H
3 
4 #include "RobotKinematics3D.h"
5 #include <KrisLibrary/structs/array2d.h>
6 
33 {
34 public:
35  void Initialize(int numBodies);
36  void InitializeRigidObject();
37  void Merge(const std::vector<RobotDynamics3D*>& robots);
38  void Subset(const RobotDynamics3D& robot,const std::vector<int>& subset);
39  void UpdateDynamics();
40  void Update_J();
41  void Update_dB_dq();
42 
44  bool InVelocityLimits(const Vector& dq) const;
45  bool InTorqueLimits(const Vector& torques) const;
46  bool InPowerLimits(const Vector& dq,const Vector& torques) const;
47 
49  bool IsUnactuatedLink(int i) const { return torqueMax(i)==0 || powerMax(i)==0; }
50  bool IsActuatedLink(int i) const { return !IsUnactuatedLink(i); }
51 
54  bool GetJacobianDt(const Vector3& pi, int i, int j, Vector3&dtheta_dt,Vector3& dp_dt) const;
56  void GetWorldAcceleration(const Vector3& pi, int i, const Vector& ddq, Vector3& dw,Vector3& dv) const;
58  void GetResidualAcceleration(const Vector3& pi, int i, Vector3& dw,Vector3& dv) const;
59 
61  Vector3 GetLinearMomentum(int i) const;
63  Vector3 GetLinearMomentum() const;
65  Vector3 GetAngularMomentum(int i) const;
69  Real GetKineticEnergy(int i) const;
71  Real GetKineticEnergy() const;
72 
74  void GetKineticEnergyMatrix(Matrix& B) const;
76  Real GetKineticEnergyDeriv(int i,int j,int z) const;
78  void GetKineticEnergyMatrixDeriv(int z,Matrix& dB) const;
80  void GetKineticEnergyMatrixTimeDeriv(Matrix& dB) const;
82  void GetCoriolisForceMatrix(Matrix& C);
84  void GetCoriolisForces(Vector& Cdq);
85 
86  //B*ddq + C*dq = fext
87  void CalcAcceleration(Vector& ddq, const Vector& fext);
88  void CalcTorques(const Vector& ddq, Vector& fext);
89 
90  Vector dq;
91  Vector velMin,velMax;
92  Vector torqueMax;
93  Vector powerMax;
94 
95  //temp storage
98  std::vector<Matrix> dB_dq;
99 };
100 
101 
102 #endif
bool GetJacobianDt(const Vector3 &pi, int i, int j, Vector3 &dtheta_dt, Vector3 &dp_dt) const
Definition: RobotDynamics3D.cpp:119
bool IsUnactuatedLink(int i) const
Query helpers.
Definition: RobotDynamics3D.h:49
void Update_J()
Updates JO and JP.
Definition: RobotDynamics3D.cpp:73
A 3D vector class.
Definition: math3d/primitives.h:136
Vector3 GetAngularMomentum() const
Angular momentum of robot.
Definition: RobotDynamics3D.cpp:198
void GetWorldAcceleration(const Vector3 &pi, int i, const Vector &ddq, Vector3 &dw, Vector3 &dv) const
Given some ddq, gets the acceleration of point pi on link i.
Definition: RobotDynamics3D.cpp:135
bool InVelocityLimits(const Vector &dq) const
Check physical limits.
Definition: RobotDynamics3D.cpp:96
Vector velMax
velocity limits
Definition: RobotDynamics3D.h:91
void GetKineticEnergyMatrixDeriv(int z, Matrix &dB) const
Computes the derivative of B with respect to q(z)
Definition: RobotDynamics3D.cpp:311
void GetCoriolisForceMatrix(Matrix &C)
Computes the coriolis force matrix C (such that C*dq = coriolis forces)
Definition: RobotDynamics3D.cpp:403
void Update_dB_dq()
Updates dB_dq. Called internally by GetCoriolis*()
Definition: RobotDynamics3D.cpp:87
Real GetKineticEnergy() const
Kinetic energy of robot.
Definition: RobotDynamics3D.cpp:221
void UpdateDynamics()
This calls UpdateFrames() and Update_J()
Definition: RobotDynamics3D.cpp:67
Vector dq
current velocity
Definition: RobotDynamics3D.h:90
void GetResidualAcceleration(const Vector3 &pi, int i, Vector3 &dw, Vector3 &dv) const
Given no joint acceleration, gets the acceleration of point pi on link i.
Definition: RobotDynamics3D.cpp:149
Array2D< Vector3 > JO
derivative of orientation[i] w.r.t. q(j)
Definition: RobotDynamics3D.h:97
void GetCoriolisForces(Vector &Cdq)
Computes the coriolis forces.
Definition: RobotDynamics3D.cpp:422
void GetKineticEnergyMatrix(Matrix &B) const
Computes the kinetic energy matrix B.
Definition: RobotDynamics3D.cpp:229
Vector powerMax
Power=|torque||velocity| limits.
Definition: RobotDynamics3D.h:93
Vector3 GetLinearMomentum() const
Linear momentum of robot.
Definition: RobotDynamics3D.cpp:179
A finite subset of numbered items with convenient operators for union, intersection, difference, etc.
Definition: Subset.h:13
Array2D< Vector3 > JP
derivative of cm[i] w.r.t. q(j)
Definition: RobotDynamics3D.h:96
void GetKineticEnergyMatrixTimeDeriv(Matrix &dB) const
Computes dB/dt.
Definition: RobotDynamics3D.cpp:393
std::vector< Matrix > dB_dq
derivative of the kinetic energy matrix w.r.t q(i)
Definition: RobotDynamics3D.h:98
Class defining kinematic and dynamic state of a robot, with commonly used functions. Inherits from RobotKinematics3D.
Definition: RobotDynamics3D.h:32
Class defining kinematics of a robot, and commonly used functions.
Definition: RobotKinematics3D.h:33
Vector torqueMax
torque limits
Definition: RobotDynamics3D.h:92
Real GetKineticEnergyDeriv(int i, int j, int z) const
Computes dBij/dqz.
Definition: RobotDynamics3D.cpp:273