Dynamics simulation handling class. More...
#include <dynamics_sim.h>
Public Member Functions | |
Dynamics (Robot_basic *robot_) | |
Constructor. | |
virtual void | plot () |
Virtual plot functions. | |
void | reset_time () |
Set the simulation time to the inital time. | |
void | Runge_Kutta4 () |
Runge Kutta solver. | |
void | Runge_Kutta4_Real_time () |
Runge Kutta solver for real time. | |
short | set_controller (const Control_Select &x) |
Set the control variable from the Control_Select reference. | |
void | set_dof (Robot_basic *robot_) |
Set the degree of freedom. | |
void | set_final_time (const double tf) |
Set the file time. | |
ReturnMatrix | set_robot_on_first_point_of_splines () |
Set the robot on first point of trajectory. | |
void | set_time_frame (const int nsteps) |
Set the number of iterations. | |
short | set_trajectory (const Trajectory_Select &x) |
Set the path_select variable from the Trajectory_Select reference. | |
ReturnMatrix | xdot (const Matrix &xin) |
Obtain state derivative. | |
virtual | ~Dynamics () |
Static Public Member Functions | |
static Dynamics * | Instance () |
A pointer to Dynamics instance. Pointer is 0 if there is no instance (logic done in Constructor). | |
Public Attributes | |
Control_Select | control |
Instance of Control_Select class. | |
int | dof_fix |
Degree of freedom + virtual link. | |
double | dt |
Time frame. | |
bool | first_pass_Kutta |
First time in all Runge_Kutta4 functions. | |
double | h |
Runge Kutta temporary variable. | |
double | h2 |
Runge Kutta temporary variable. | |
Matrix | k1 |
Runge Kutta temporary variable. | |
Matrix | k2 |
Runge Kutta temporary variable. | |
Matrix | k3 |
Runge Kutta temporary variable. | |
Matrix | k4 |
Runge Kutta temporary variable. | |
int | ndof |
Degree of freedom. | |
int | nsteps |
Numbers of iterations between. | |
Trajectory_Select | path_select |
Instance of Trajectory_Select class. | |
ColumnVector | pd |
Desired end effector cartesian position. | |
ColumnVector | ppd |
Desired end effector cartesian velocity. | |
ColumnVector | pppd |
Desired end effector cartesian acceleration. | |
ColumnVector | q |
Joints positions. | |
ColumnVector | qd |
Desired joints positions. | |
ColumnVector | qp |
Joints velocities. | |
ColumnVector | qpd |
Desired joints velocities. | |
ColumnVector | qpp |
Joints accelerations. | |
ColumnVector | qppd |
Desired joints accelerations. | |
Quaternion | quatd |
Desired orientation express by a quaternion. | |
Robot_basic * | robot |
Pointer on Robot_basic class. | |
ColumnVector | tau |
Controller output torque. | |
double | tf |
Final time used in Runge_Kutta4_Real_time. | |
double | tf_cont |
Final time used in Runge_Kutta4. | |
double | time |
Time during simulation. | |
double | to |
Initial simulation time. | |
ColumnVector | wd |
Desired end effector cartesian angular velocity. | |
ColumnVector | wpd |
Desired end effector cartesian angular acceleration. | |
Matrix | x |
Stated vector obtain in Runge Kutta functions. | |
Matrix | xd |
Statd vector derivative obtaint in xdot function. | |
Static Public Attributes | |
static Dynamics * | instance = 0 |
Static pointer on Dynamics class. |
Dynamics simulation handling class.
Definition at line 60 of file dynamics_sim.h.
Dynamics::Dynamics | ( | Robot_basic * | robot_ | ) |
Constructor.
Definition at line 57 of file dynamics_sim.cpp.
virtual Dynamics::~Dynamics | ( | ) | [inline, virtual] |
Definition at line 65 of file dynamics_sim.h.
Dynamics * Dynamics::Instance | ( | ) | [static] |
A pointer to Dynamics instance. Pointer is 0 if there is no instance (logic done in Constructor).
Definition at line 100 of file dynamics_sim.cpp.
virtual void Dynamics::plot | ( | ) | [inline, virtual] |
Virtual plot functions.
Reimplemented in New_dynamics.
Definition at line 76 of file dynamics_sim.h.
void Dynamics::reset_time | ( | ) |
Set the simulation time to the inital time.
Definition at line 155 of file dynamics_sim.cpp.
void Dynamics::Runge_Kutta4 | ( | ) |
Runge Kutta solver.
Definition at line 356 of file dynamics_sim.cpp.
void Dynamics::Runge_Kutta4_Real_time | ( | ) |
Runge Kutta solver for real time.
Definition at line 334 of file dynamics_sim.cpp.
short Dynamics::set_controller | ( | const Control_Select & | x | ) |
Set the control variable from the Control_Select reference.
Definition at line 161 of file dynamics_sim.cpp.
void Dynamics::set_dof | ( | Robot_basic * | robot_ | ) |
Set the degree of freedom.
Obtain the degree of freedom from Robot_basic pointer. Some vectors will be resize with new current dof value.
Definition at line 109 of file dynamics_sim.cpp.
void Dynamics::set_final_time | ( | const double | tf | ) |
Set the file time.
Definition at line 149 of file dynamics_sim.cpp.
ReturnMatrix Dynamics::set_robot_on_first_point_of_splines | ( | ) |
Set the robot on first point of trajectory.
Assigned the robot joints position to the first point of the trajectory if the latter is expressed in joint space, or assigned the robot joints position via inverse kinematics if the trajectory is expressed in cartesian space. The function return a message on the console if the format of the trajectory file is incorrect.
Definition at line 190 of file dynamics_sim.cpp.
void Dynamics::set_time_frame | ( | const int | nsteps | ) |
Set the number of iterations.
Definition at line 143 of file dynamics_sim.cpp.
short Dynamics::set_trajectory | ( | const Trajectory_Select & | x | ) |
Set the path_select variable from the Trajectory_Select reference.
Definition at line 174 of file dynamics_sim.cpp.
ReturnMatrix Dynamics::xdot | ( | const Matrix & | x | ) |
Obtain state derivative.
x,: | state vector (joint speed and joint velocity). |
The controller torque is applied if any controller has been selected, then the joint acceleration is obtained.
Definition at line 252 of file dynamics_sim.cpp.
Instance of Control_Select class.
Definition at line 111 of file dynamics_sim.h.
Degree of freedom + virtual link.
Definition at line 82 of file dynamics_sim.h.
double Dynamics::dt |
Time frame.
Definition at line 85 of file dynamics_sim.h.
First time in all Runge_Kutta4 functions.
Definition at line 81 of file dynamics_sim.h.
double Dynamics::h |
Runge Kutta temporary variable.
Definition at line 85 of file dynamics_sim.h.
double Dynamics::h2 |
Runge Kutta temporary variable.
Definition at line 85 of file dynamics_sim.h.
Dynamics * Dynamics::instance = 0 [static] |
Static pointer on Dynamics class.
Definition at line 115 of file dynamics_sim.h.
Matrix Dynamics::k1 |
Runge Kutta temporary variable.
Definition at line 92 of file dynamics_sim.h.
Matrix Dynamics::k2 |
Runge Kutta temporary variable.
Definition at line 92 of file dynamics_sim.h.
Matrix Dynamics::k3 |
Runge Kutta temporary variable.
Definition at line 92 of file dynamics_sim.h.
Matrix Dynamics::k4 |
Runge Kutta temporary variable.
Definition at line 92 of file dynamics_sim.h.
int Dynamics::ndof |
Degree of freedom.
Definition at line 82 of file dynamics_sim.h.
int Dynamics::nsteps |
Numbers of iterations between.
Definition at line 82 of file dynamics_sim.h.
Instance of Trajectory_Select class.
Definition at line 112 of file dynamics_sim.h.
ColumnVector Dynamics::pd |
Desired end effector cartesian position.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::ppd |
Desired end effector cartesian velocity.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::pppd |
Desired end effector cartesian acceleration.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::q |
Joints positions.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::qd |
Desired joints positions.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::qp |
Joints velocities.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::qpd |
Desired joints velocities.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::qpp |
Joints accelerations.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::qppd |
Desired joints accelerations.
Definition at line 98 of file dynamics_sim.h.
Desired orientation express by a quaternion.
Definition at line 110 of file dynamics_sim.h.
Pointer on Robot_basic class.
Reimplemented in New_dynamics.
Definition at line 113 of file dynamics_sim.h.
ColumnVector Dynamics::tau |
Controller output torque.
Definition at line 98 of file dynamics_sim.h.
double Dynamics::tf |
Final time used in Runge_Kutta4_Real_time.
Definition at line 85 of file dynamics_sim.h.
double Dynamics::tf_cont |
Final time used in Runge_Kutta4.
Definition at line 85 of file dynamics_sim.h.
double Dynamics::time |
Time during simulation.
Definition at line 85 of file dynamics_sim.h.
double Dynamics::to |
Initial simulation time.
Definition at line 85 of file dynamics_sim.h.
ColumnVector Dynamics::wd |
Desired end effector cartesian angular velocity.
Definition at line 98 of file dynamics_sim.h.
ColumnVector Dynamics::wpd |
Desired end effector cartesian angular acceleration.
Definition at line 98 of file dynamics_sim.h.
Matrix Dynamics::x |
Stated vector obtain in Runge Kutta functions.
Definition at line 92 of file dynamics_sim.h.
Matrix Dynamics::xd |
Statd vector derivative obtaint in xdot function.
Definition at line 92 of file dynamics_sim.h.