SCL
1.0
Standard Control Library : Control, dynamics, physics, and simulation
|
#include <CSimbodyDynamics.hpp>
Public Member Functions | |
virtual bool | init (const SRobotParsed &arg_robot_data) |
virtual bool | computeGCModel (SRobotSensors const *sensor_data, SGcModel *js_model) |
virtual const void * | getIdForLink (std::string arg_link_name) |
virtual sBool | computeTransform_Depracated (const void *arg_link_id, Eigen::Affine3d &arg_T) |
virtual sBool | computeJacobian_Depracated (const void *arg_link_id, const Eigen::VectorXd &arg_pos_global, Eigen::MatrixXd &arg_J) |
virtual sBool | integrate (SRobotIO &arg_inputs, const sFloat arg_time_interval) |
virtual sFloat | getKineticEnergy_Depracated () |
virtual sFloat | getPotentialEnergy_Depracated () |
CSimbodyDynamics () | |
virtual | ~CSimbodyDynamics () |
virtual sBool | computeTransformsForAllLinks (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q) |
virtual sBool | computeTransform (SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q) |
virtual sBool | computeTransformToAncestor (Eigen::Affine3d &arg_T, SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q) |
virtual sBool | computeJacobianWithTransforms (Eigen::MatrixXd &arg_J, SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) |
virtual sBool | computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) const |
virtual sBool | computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) |
virtual sBool | computeJacobianTrans (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) |
virtual sBool | computeJacobianRot (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) |
virtual sBool | computeExternalContacts (sutil::CMappedList< std::string, SForce > &arg_contacts) |
virtual sFloat | computeEnergyKinetic (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q, const Eigen::VectorXd &arg_dq) |
virtual sFloat | computeEnergyPotential (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q) |
virtual sBool | hasBeenInit () |
Protected Attributes | |
sBool | has_been_init_ |
const SRobotParsed * | robot_parsed_data_ |
Private Member Functions | |
sBool | addNonRootLink (SimTK::MobilizedBody &arg_parent, const SRigidBody &arg_child_lnk) |
Private Attributes | |
std::string | robot_name_ |
size_t | ndof_ |
Eigen::Vector3d | gravity_ |
SimTK::MultibodySystem | simbody_system_ |
SimTK::SimbodyMatterSubsystem | simbody_matter_ |
SimTK::GeneralForceSubsystem | simbody_forces_ |
SimTK::RungeKuttaMersonIntegrator * | simbody_rkm_integ_ |
SimTK::TimeStepper * | simbody_ts_ |
SimTK::State | simbody_state_ |
Computes the dynamics of a robot model using the Simbody dynamics engine
scl::CSimbodyDynamics::CSimbodyDynamics | ( | ) |
Constructor : Sets defaults
|
virtual |
Destructor : Deletes stuff
|
private |
Adds a non-root link
|
inlinevirtualinherited |
Gets the robot's kinetic energy
arg_tree | The tree for which the transformation matrices are to be updated |
arg_q | The current generalized coordinates. |
arg_dq | The current generalized velocities. |
Reimplemented in scl::CDynamicsScl, scl_ext::CDynamics3d, and scl_ext::CDynamicsSclSpatial.
|
inlinevirtualinherited |
Gets the robot's potential energy
arg_tree | The tree for which the transformation matrices are to be updated |
arg_q | The current generalized coordinates. |
Reimplemented in scl::CDynamicsScl, scl_ext::CDynamicsSclSpatial, and scl_ext::CDynamics3d.
|
inlinevirtualinherited |
Calculates the collision forces for the robot to which this dynamics object is assigned.
Uses std::string based force lookup. The dynamics implementation should support this (maintain a map or something).
NOTE : The dynamics engine may delete forces from the passed mapped list at will. Do not store pointers to them at random places in your code.
As of now, this is optional for dynamics engines
arg_contacts | This is where the simulator will store the contact forces. It may use the std::string to identify when to remove or re-add forces. |
Reimplemented in scl_ext::CDynamics3d.
|
virtual |
Updates the generalized coordinate model matrices (Everything in SGcModel).
This is the most efficient method to access the standard matrices. Computing transformations and Jacobians individually is typically wasteful.
arg_sensor_data | This is where there current robot state is read when updateModelMatrices() is called. |
arg_gc_model | Pointer to the joint-space model structure for the robot. This is the place where the matrices reside that will get updated when calling updateModelMatrices(). |
Implements scl::CDynamicsBase.
|
inlinevirtualinherited |
Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link. ** CONST VERSION : NOTE : The regular version should call this. **
arg_J | The Jacobain will be saved here. |
arg_link | The link at which the Jacobian is to be calculated |
arg_q | The current generalized coordinates. |
arg_pos_local | The offset from the link's frame (in link coordinates). |
Reimplemented in scl::CDynamicsScl.
|
inlinevirtualinherited |
Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_ancestor_coords = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.
arg_J | The Jacobain will be saved here. |
arg_link | The link at which the Jacobian is to be calculated |
arg_ancestor | The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root. |
arg_q | The current generalized coordinates. |
arg_pos_local | The offset from the link's frame (in link coordinates). |
|
virtual |
Calculates the Jacobian for the robot to which this dynamics object is assigned.
The Jacobian is specified by a link and an offset (in task space dimensions)from that link
Uses id based link lookup. The dynamics implementation should support this (maintain a map or something).
arg_link_id | The link at which the Jacobian is to be calculated |
arg_pos_global | The offset from the link's frame. |
arg_J | The Jacobain will be saved here. |
|
inlinevirtualinherited |
Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for rotation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq
arg_J | The Jacobain will be saved here. |
arg_link | The link at which the Jacobian is to be calculated |
arg_ancestor | The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root. |
arg_q | The current generalized coordinates. |
arg_pos_local | The offset from the link's frame (in link coordinates). |
|
inlinevirtualinherited |
Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for translation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq
arg_J | The Jacobain will be saved here. |
arg_link | The link at which the Jacobian is to be calculated |
arg_ancestor | The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root. |
arg_q | The current generalized coordinates. |
arg_pos_local | The offset from the link's frame (in link coordinates). |
|
inlinevirtualinherited |
Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.
arg_J | The Jacobain will be saved here. |
arg_link | The link at which the Jacobian is to be calculated. Transforms are updated in this link and ancestors in the tree. |
arg_q | The current generalized coordinates. |
arg_pos_local | The offset from the link's frame (in link coordinates). |
Reimplemented in scl::CDynamicsScl.
|
inlinevirtualinherited |
Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_parent_link_coords = arg_link.T_lnk_ * x_link_coords Note that the matrix is stored in the passed link data struct.
arg_link | The link at which the transformation matrix is to be calculated |
arg_q | The current generalized coordinates. |
Reimplemented in scl::CDynamicsScl.
|
virtual |
Calculates the Transformation Matrix for the robot to which this dynamics object is assigned.
The Transformation Matrix is specified by a link and an offset (in task space dimensions)from that link and is given by:
x_global_coords = T * x_link_coords
Uses id based link lookup. The dynamics implementation should support this (maintain a map or something).
arg_link_id | The link at which the transformation matrix is to be calculated |
arg_T | The transformation matrix will be saved here. |
|
inlinevirtualinherited |
Updates the Transformation Matrices for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords
arg_tree | The tree for which the transformation matrices are to be updated |
arg_q | The current generalized coordinates. |
Reimplemented in scl::CDynamicsScl.
|
inlinevirtualinherited |
Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords
Note that the local transformation matrices are updated in all the link data structs from arg_link to arg_ancestor.
Also note that there are two origins, robot origin, which actually has a node allocated for it. And the global scl origin, for which you should pass in a NULL as ancestor.
arg_T | The transformation matrix in which to store the answer |
arg_link | The link at which the transformation matrix is to be calculated |
arg_ancestor | The link up to which the transformation matrix is to be calculated Pass NULL to compute the transform up to the global root. |
arg_q | The current generalized coordinates. |
Reimplemented in scl::CDynamicsScl.
|
virtual |
Gives an id for a link name.
Enables using calculateJacobain without:
|
virtual |
Gets the robot's kinetic energy
|
virtual |
Gets the robot's potential energy
|
inlinevirtualinherited |
Initialization state
|
virtual |
Initialize the CSimbodyDynamics with the Simbody trees it needs in order to update the kinematics and/or dynamics of a robot.
Accesses the database.
Implements scl::CDynamicsBase.
|
virtual |
Integrates the robot's state. Uses the given applied forces, torques, positions and velocities and its internal dynamic model to compute new positions and velocities.
This version performs the entire set of operations on the SRobotIO data structure.
Reads from, and updates: arg_inputs_.sensors_.q_ arg_inputs_.sensors_.dq_ arg_inputs_.sensors_.ddq_
Reads from: arg_inputs_.actuators_.forces_ arg_inputs_.actuators_.force_gc_commanded_
arg_inputs | The existing generalized coordinates, velocities and accelerations + The generalized forces + task (euclidean) forces and the list of contact points and links. |
arg_time_interval | The time across which the system should integrate the dynamics. |
Simbody uses a forward euler integrator and uses this as the dt so set it to a small value.
Reimplemented from scl::CDynamicsBase.
|
private |
The gravity acting on the robot
|
protectedinherited |
True if the dynamics object has been initialized for a given robot
|
private |
The robot's degrees of freedom
|
private |
The robot's name
|
protectedinherited |
A read-only pointer to access parsed data
|
private |
Simbody's classes