opensurgsim
|
The RigidRepresentation class defines the dynamic rigid body representation Note that the rigid representation is velocity-based, therefore its degrees of freedom are the linear and angular velocities: 6 Dof. More...
#include <RigidRepresentation.h>
Public Member Functions | |
RigidRepresentation (const std::string &name) | |
Constructor. More... | |
virtual | ~RigidRepresentation () |
Destructor. | |
SURGSIM_CLASSNAME (SurgSim::Physics::RigidRepresentation) | |
void | setLinearVelocity (const SurgSim::Math::Vector3d &linearVelocity) |
Set the current linear velocity of the rigid representation. More... | |
void | setAngularVelocity (const SurgSim::Math::Vector3d &angularVelocity) |
Set the current angular velocity of the rigid representation. More... | |
void | addExternalGeneralizedForce (const SurgSim::Math::Vector6d &generalizedForce, const SurgSim::Math::Matrix66d &K=SurgSim::Math::Matrix66d::Zero(), const SurgSim::Math::Matrix66d &D=SurgSim::Math::Matrix66d::Zero()) |
Add an external generalized force applied to the rigid representation's mass center. More... | |
void | addExternalGeneralizedForce (const SurgSim::DataStructures::Location &location, const SurgSim::Math::Vector6d &generalizedForce, const SurgSim::Math::Matrix66d &K=SurgSim::Math::Matrix66d::Zero(), const SurgSim::Math::Matrix66d &D=SurgSim::Math::Matrix66d::Zero()) |
Add an external generalized force applied to the rigid representation (anywhere). More... | |
SurgSim::DataStructures::BufferedValue< SurgSim::Math::Vector6d > & | getExternalGeneralizedForce () |
const SurgSim::Math::Matrix66d & | getExternalGeneralizedStiffness () const |
const SurgSim::Math::Matrix66d & | getExternalGeneralizedDamping () const |
void | beforeUpdate (double dt) override |
Preprocessing done before the update call This needs to be called from the outside usually from a Computation. More... | |
void | update (double dt) override |
Update the representation state to the current time step. More... | |
void | afterUpdate (double dt) override |
Postprocessing done after the update call This needs to be called from the outside usually from a Computation. More... | |
void | applyCorrection (double dt, const Eigen::VectorBlock< SurgSim::Math::Vector > &deltaVelocity) override |
Update the Representation's current position and velocity using a time interval, dt, and change in velocity, deltaVelocity. More... | |
const SurgSim::Math::Matrix66d & | getComplianceMatrix () const |
Retrieve the rigid body 6x6 compliance matrix. More... | |
![]() | |
RigidRepresentationBase (const std::string &name) | |
Constructor. More... | |
virtual | ~RigidRepresentationBase () |
Destructor. | |
void | setInitialState (const RigidState &state) |
Set the initial state of the rigid representation. More... | |
void | resetState () override |
Reset the representation to its initial/default state. | |
const RigidState & | getInitialState () const |
Get the initial state of the rigid representation. More... | |
const RigidState & | getCurrentState () const |
Get the current state of the rigid representation. More... | |
const RigidState & | getPreviousState () const |
Get the previous state of the rigid representation. More... | |
std::shared_ptr< Localization > | createLocalization (const SurgSim::DataStructures::Location &location) override |
Computes a localized coordinate w.r.t this representation, given a Location object. More... | |
void | setDensity (double rho) |
Set the mass density of the rigid representation. More... | |
double | getDensity () const |
Get the mass density of the rigid representation. More... | |
double | getMass () const |
Get the mass of the rigid body. More... | |
const SurgSim::Math::Vector3d & | getMassCenter () const |
Get the mass center of the rigid body. More... | |
const SurgSim::Math::Matrix33d & | getLocalInertia () const |
Get the local inertia 3x3 matrix of the rigid body. More... | |
void | setLinearDamping (double linearDamping) |
Set the linear damping parameter. More... | |
double | getLinearDamping () const |
Get the linear damping parameter. More... | |
void | setAngularDamping (double angularDamping) |
Set the angular damping parameter. More... | |
double | getAngularDamping () const |
Get the angular damping parameter. More... | |
void | setShape (const std::shared_ptr< SurgSim::Math::Shape > shape) |
Set the shape to use internally for physical parameters computation. More... | |
const std::shared_ptr< SurgSim::Math::Shape > | getShape () const |
Get the shape used internally for physical parameters computation. More... | |
void | setCollisionRepresentation (std::shared_ptr< SurgSim::Collision::Representation > representation) override |
Set the collision representation for this physics representation, when the collision object is involved in a collision, the collision should be resolved inside the dynamics calculation. More... | |
void | beforeUpdate (double dt) override |
Preprocessing done before the update call This needs to be called from the outside usually from a Computation. More... | |
void | afterUpdate (double dt) override |
Postprocessing done after the update call This needs to be called from the outside usually from a Computation. More... | |
![]() | |
Representation (const std::string &name) | |
Constructor. More... | |
virtual | ~Representation () |
Destructor. | |
size_t | getNumDof () const |
Query the object number of degrees of freedom. More... | |
void | setIsGravityEnabled (bool isGravityEnabled) |
Set the gravity enable flag. More... | |
bool | isGravityEnabled () const |
Get the gravity enable flag. More... | |
void | setIsDrivingSceneElementPose (bool isDrivingSceneElementPose) |
Set whether this Representation is controlling the pose of the SceneElement that it is part of. More... | |
bool | isDrivingSceneElementPose () |
Query if this Representation is controlling the pose of the SceneElement that it is part of. More... | |
std::shared_ptr< SurgSim::Collision::Representation > | getCollisionRepresentation () const |
std::shared_ptr< ConstraintImplementation > | getConstraintImplementation (SurgSim::Physics::ConstraintType type) |
Get a constraint implementation of the given type for this representation. More... | |
![]() | |
Representation (const std::string &name) | |
Constructor. More... | |
virtual void | setLocalPose (const SurgSim::Math::RigidTransform3d &pose) |
Set the pose of the representation with respect to the Scene Element. More... | |
virtual SurgSim::Math::RigidTransform3d | getLocalPose () const |
Get the pose of the representation with respect to the Scene Element. More... | |
virtual SurgSim::Math::RigidTransform3d | getPose () const |
Get the pose of the representation in world coordinates. More... | |
![]() | |
Component (const std::string &name) | |
Constructor. More... | |
virtual | ~Component () |
Destructor. | |
std::string | getName () const |
Gets component name. More... | |
std::string | getFullName () const |
Gets a string containing the name of the Component and (if it has one) its SceneElement. More... | |
void | setName (const std::string &name) |
Sets the name of component. More... | |
boost::uuids::uuid | getUuid () const |
Gets the id of the component. | |
bool | isInitialized () const |
bool | initialize (const std::weak_ptr< Runtime > &runtime) |
Initialize this component, this needs to be called before wakeUp() can be called. More... | |
bool | isAwake () const |
bool | wakeUp () |
Wakeup this component, this will be called when the component is inserted into the ComponentManager that is responsible for handling this component. More... | |
void | retire () |
Retire this component, this will be called when the component is removed from the ComponentManager that is responsible for handling this component. More... | |
void | setScene (std::weak_ptr< Scene > scene) |
Sets the scene. More... | |
std::shared_ptr< Scene > | getScene () |
Gets the scene. More... | |
void | setSceneElement (std::weak_ptr< SceneElement > sceneElement) |
Sets the scene element. More... | |
std::shared_ptr< SceneElement > | getSceneElement () |
Gets the scene element. More... | |
std::shared_ptr< const SceneElement > | getSceneElement () const |
Gets the scene element, constant version. More... | |
std::shared_ptr< Runtime > | getRuntime () const |
Get the runtime which contains this component. More... | |
virtual std::string | getClassName () const |
The class name for this class, this being the base class it should return SurgSim::Framework::Component but this would make missing implemenentations of this hard to catch, therefore this calls SURGSIM_FAILURE. More... | |
std::shared_ptr< Component > | getSharedPtr () |
Gets a shared pointer to this component. More... | |
virtual void | doRetire () |
Interface to be implemented by derived classes Has a default implementation, does nothing. | |
bool | isActive () const |
virtual void | setLocalActive (bool val) |
Set the component's active state. More... | |
bool | isLocalActive () const |
![]() | |
Accessible () | |
Default Constructor. | |
~Accessible () | |
Destructor. | |
template<class T > | |
T | getValue (const std::string &name) const |
Retrieves the value with the name by executing the getter if it is found and tries to convert it to the given type. More... | |
boost::any | getValue (const std::string &name) const |
Retrieves the value with the name by executing the getter if it is found. More... | |
template<class T > | |
bool | getValue (const std::string &name, T *value) const |
Retrieves the value with the name by executing the getter if it is found, and converts it to the type of the output parameter. More... | |
void | setValue (const std::string &name, const boost::any &value) |
Sets a value of a property that has setter. More... | |
bool | isReadable (const std::string &name) const |
Check whether a property is readable. More... | |
bool | isWriteable (const std::string &name) const |
Check whether a property is writable. More... | |
void | setGetter (const std::string &name, GetterType func) |
Sets a getter for a given property. More... | |
void | setSetter (const std::string &name, SetterType func) |
Sets a setter for a given property. More... | |
void | setAccessors (const std::string &name, GetterType getter, SetterType setter) |
Sets the accessors getter and setter in one function. More... | |
void | removeAccessors (const std::string &name) |
Removes all the accessors (getter and setter) for a given property. More... | |
void | forwardProperty (const std::string &name, const Accessible &target, const std::string &targetProperty) |
Adds a property with the given name that uses the targets accessors, in effect forwarding the value to the target. More... | |
void | setSerializable (const std::string &name, EncoderType encoder, DecoderType decoder) |
Sets the functions used to convert data from and to a YAML::Node. More... | |
void | setDecoder (const std::string &name, DecoderType decoder) |
Sets the functions used to convert data from a YAML::Node. More... | |
YAML::Node | encode () const |
Encode this Accessible to a YAML::Node. More... | |
void | decode (const YAML::Node &node, const std::vector< std::string > &ignoredProperties=std::vector< std::string >()) |
Decode this Accessible from a YAML::Node, will throw an exception if the data type cannot be converted. More... | |
std::vector< std::string > | getProperties () |
template<> | |
boost::any | getValue (const std::string &name) const |
Protected Attributes | |
SurgSim::Math::Matrix33d | m_globalInertia |
Inertia matrices in global coordinates. | |
SurgSim::Math::Matrix33d | m_invGlobalInertia |
Inverse of inertia matrix in global coordinates. | |
SurgSim::Math::Vector3d | m_force |
Current force applied on the rigid representation (in N) | |
SurgSim::Math::Vector3d | m_torque |
Current torque applied on the rigid representation (in N.m) | |
SurgSim::Math::Matrix66d | m_C |
Compliance matrix (size of the number of Dof = 6) | |
bool | m_hasExternalGeneralizedForce |
External generalized force, stiffness and damping applied on the rigid representation. | |
SurgSim::DataStructures::BufferedValue< SurgSim::Math::Vector6d > | m_externalGeneralizedForce |
SurgSim::Math::Matrix66d | m_externalGeneralizedStiffness |
SurgSim::Math::Matrix66d | m_externalGeneralizedDamping |
![]() | |
RigidState | m_initialState |
Initial rigid representation state (useful for reset) | |
RigidState | m_previousState |
Previous rigid representation state. | |
RigidState | m_currentState |
Current rigid representation state. | |
RigidState | m_finalState |
Last valid/final rigid representation state. | |
bool | m_parametersValid |
Validity of the parameters. | |
double | m_rho |
Density of the object (in Kg.m-3) | |
double | m_mass |
Total mass of the object (in Kg) | |
double | m_linearDamping |
Linear damping parameter (in N.s.m-1 or Kg.s-1) | |
double | m_angularDamping |
Angular damping parameter (in N.m.s.rad-1) | |
SurgSim::Math::Vector3d | m_massCenter |
Mass-center of the object. | |
SurgSim::Math::Matrix33d | m_localInertia |
Inertia matrix in local coordinates. | |
std::shared_ptr< SurgSim::Math::Shape > | m_shape |
Shape to be used for the mass/inertia calculation. | |
![]() | |
std::shared_ptr< SurgSim::Collision::Representation > | m_collisionRepresentation |
This entity's collision representation, these are usually very specific to the physics representation. | |
std::shared_ptr< SurgSim::Framework::Logger > | m_logger |
Logger for this class. | |
Additional Inherited Members | |
![]() | |
typedef std::function< boost::any(void)> | GetterType |
typedef std::function< void(boost::any)> | SetterType |
typedef std::function< YAML::Node(void)> | EncoderType |
typedef std::function< void(const YAML::Node *)> | DecoderType |
![]() | |
typedef ObjectFactory1< Component, std::string > | FactoryType |
![]() | |
static FactoryType & | getFactory () |
![]() | |
bool | doInitialize () override |
Interface to be implemented by derived classes. More... | |
bool | doWakeUp () override |
Interface to be implemented by derived classes. More... | |
template<class T > | |
std::shared_ptr< T > | createTypedLocalization (const SurgSim::DataStructures::Location &location) |
Creates typed localization. More... | |
![]() | |
void | setNumDof (size_t numDof) |
Set the number of degrees of freedom. More... | |
const SurgSim::Math::Vector3d & | getGravity () const |
Get the gravity used by this Representation. More... | |
void | driveSceneElementPose (const SurgSim::Math::RigidTransform3d &pose) |
This conditionally updates that pose for the scenelement to the given pose The update gets exectuded if the representation actually has sceneelement and isDrivingScenElement() is true. More... | |
![]() | |
virtual std::shared_ptr< PoseComponent > | getPoseComponent () |
Get the PoseComponent for this component. More... | |
virtual std::shared_ptr< const PoseComponent > | getPoseComponent () const |
Get the PoseComponent for this component, constant access. More... | |
The RigidRepresentation class defines the dynamic rigid body representation Note that the rigid representation is velocity-based, therefore its degrees of freedom are the linear and angular velocities: 6 Dof.
|
explicit |
Constructor.
name | The rigid representation's name |
void SurgSim::Physics::RigidRepresentation::addExternalGeneralizedForce | ( | const SurgSim::Math::Vector6d & | generalizedForce, |
const SurgSim::Math::Matrix66d & | K = SurgSim::Math::Matrix66d::Zero() , |
||
const SurgSim::Math::Matrix66d & | D = SurgSim::Math::Matrix66d::Zero() |
||
) |
Add an external generalized force applied to the rigid representation's mass center.
generalizedForce | The external generalized force to apply at the mass center |
K | The stiffness matrix associated with the generalized force (jacobian of the force w.r.t position) |
D | The damping matrix associated with the generalized force (jacobian of the force w.r.t velocity) |
void SurgSim::Physics::RigidRepresentation::addExternalGeneralizedForce | ( | const SurgSim::DataStructures::Location & | location, |
const SurgSim::Math::Vector6d & | generalizedForce, | ||
const SurgSim::Math::Matrix66d & | K = SurgSim::Math::Matrix66d::Zero() , |
||
const SurgSim::Math::Matrix66d & | D = SurgSim::Math::Matrix66d::Zero() |
||
) |
Add an external generalized force applied to the rigid representation (anywhere).
location | The application point (must contain a rigid local position) |
generalizedForce | The external generalized force |
K | The stiffness matrix associated with generalizedForce (jacobian w.r.t position) |
D | The damping matrix associated with generalizedForce (jacobian w.r.t velocity) |
The location produces an extra torque and extra terms in the derivatives (stiffness/damping matrices) coming from this extra torque.
Let's note the position dof ( \(\mathbf{C}\), \(\mathbf{W}\)) and the velocity dof ( \(\mathbf{v}\), \(\mathbf{w}\)), and \(R\) the 3x3 rotation matrix dependent on \(\mathbf{W}\). The force ( \(\mathbf{F}\)) applied on a point ( \(\mathbf{P}\)) that is not the mass center ( \(\mathbf{C}\)), produces a torque ( \(\mathbf{T}\)) and also affect matrix derivatives ( \(K\) and \(D\)) as follow:
\[ \mathbf{T} = \mathbf{CP} \wedge \mathbf{F} \]
Note that \(\mathbf{CP}\) does not depend on the velocity (neither \(\mathbf{v}\) nor \(\mathbf{w}\), therefore the damping matrix is affected by the terms:
\[ \displaystyle -\frac{\partial \mathbf{T}}{\partial \mathbf{v}} = -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{v}} = \mathbf{CP} \wedge D_{3 \times 3}^{(0, 0)} \]
\[ \displaystyle -\frac{\partial \mathbf{T}}{\partial \mathbf{w}} = -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{w}} = \mathbf{CP} \wedge D_{3 \times 3}^{(0, 3)} \]
Also note that \(\mathbf{CP}\) does not change if the rigid translate ( \(\mathbf{C}\) changes), but it changes if the rotation of the rigid changes ( \(\mathbf{W}\) changes => \(R\) changes).
Therefore \(\mathbf{CP}\) is a constant vector in the rigid local space \( \mathbf{CP} = R.\mathbf{CP_{local}} \) and its derivative w.r.t. \(\mathbf{C}\) is null and we get:
\[ \displaystyle -\frac{\partial \mathbf{T}}{\partial \mathbf{C}} = -\mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{C}} = \mathbf{CP} \wedge K_{3 \times 3}^{(0,0)} \]
\[ \displaystyle -\frac{\partial \mathbf{T}}{\partial \mathbf{W}} = -\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} \wedge \mathbf{F} - \mathbf{CP} \wedge \frac{\partial \mathbf{F}}{\partial \mathbf{W}} = -\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} \wedge \mathbf{F} + \mathbf{CP} \wedge K_{3 \times 3}^{(0, 3)} \]
To compute \(\frac{\partial \mathbf{CP}}{\partial \mathbf{W}} = \frac{\partial R}{\partial \mathbf{W}}.\mathbf{CP}_{local}\), we operate a variable change, from the rotation vector \(\mathbf{W}\) to angle/axis ( \(\theta\), \(\mathbf{u}\)) representation (4 dof):
\[ \left\{ \begin{eqnarray*} \theta = |\mathbf{W}| \\ \mathbf{u} = \frac{\mathbf{W}}{|\mathbf{W}|} \end{eqnarray*} \right. \]
In this new coordinate system, we have (see reference http://en.wikipedia.org/wiki/Rotation_matrix):
\[ R(\theta, \mathbf{u}) = cos(\theta).I + sin(\theta).\left[\mathbf{u}\right] + (1-cos(\theta)).\mathbf{u} \otimes \mathbf{u} \]
where \(\left[\mathbf{u}\right]\) denotes the skew symmetric matrix of the vector \(\mathbf{u}\), \(\left[\mathbf{u}\right] = \left( \begin{array}{ccc} 0 & -u^z & u^y \\ u^z & 0 & -u^x \\ -u^y & u^x & 0 \end{array} \right)\) and \(\mathbf{u} \otimes \mathbf{u}\) the tensor product: \(\left(\mathbf{u} \otimes \mathbf{u}^T\right)_{ij} = u^i * u^j\).
Applying the chain derivation rule, we get:
\[ \frac{\partial R}{\partial W^{\alpha}} = \frac{\partial R}{\partial \theta}.\frac{\partial \theta}{\partial W^{\alpha}} + \frac{\partial R}{\partial u^x}.\frac{\partial u^x}{\partial W^{\alpha}} + \frac{\partial R}{\partial u^y}.\frac{\partial u^y}{\partial W^{\alpha}} + \frac{\partial R}{\partial u^z}.\frac{\partial u^z}{\partial W^{\alpha}} \]
with
\[ \frac{\partial R}{\partial \theta} = -sin(\theta).I + cos(\theta).\left[\mathbf{u}\right] + sin(\theta).\mathbf{u} \otimes \mathbf{u} \]
\[ \frac{\partial R}{\partial u^x} = sin(\theta) \left( \begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & -1 \\ 0 & 1 & 0 \end{array} \right) + (1 - cos(\theta)) \left( \begin{array}{ccc} 2u^x & u^y & u^z \\ u^y & 0 & 0 \\ u^z & 0 & 0 \end{array} \right) \]
\[ \frac{\partial R}{\partial u^y} = sin(\theta) \left( \begin{array}{ccc} 0 & 0 & 1 \\ 0 & 0 & 0 \\ -1 & 0 & 0 \end{array} \right) + (1 - cos(\theta)) \left( \begin{array}{ccc} 0 & u^x & 0 \\ u^x & 2u^y & u^z \\ 0 & u^z & 0 \end{array} \right) \]
\[ \frac{\partial R}{\partial u^z} = sin(\theta) \left( \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right) + (1 - cos(\theta)) \left( \begin{array}{ccc} 0 & 0 & u^x \\ 0 & 0 & u^y \\ u^x & u^y & 2u^z \end{array} \right) \]
and
\[ \frac{\partial \theta}{\partial W^{\alpha}} = \frac{W^{\alpha}}{|\mathbf{W}|} \]
\[ \frac{\partial u}{\partial W^{\alpha}} = \frac{1}{|\mathbf{W}|} . \left( \left( \begin{array}{c} \delta^{x,\alpha} \\ \delta^{y,\alpha} \\ \delta^{z,\alpha} \end{array} \right) - \frac{W^{\alpha}}{|\mathbf{W}|} \frac{\mathbf{W}}{|\mathbf{W}|} \right) \]
|
overridevirtual |
Postprocessing done after the update call This needs to be called from the outside usually from a Computation.
dt | The time step (in seconds) |
Reimplemented from SurgSim::Physics::Representation.
|
overridevirtual |
Update the Representation's current position and velocity using a time interval, dt, and change in velocity, deltaVelocity.
This function typically is called in the physics pipeline (PhysicsManager::doUpdate) after solving the equations that enforce constraints when collisions occur. Specifically it is called in the PushResults::doUpdate step.
dt | The time step |
deltaVelocity | The block of a vector containing the correction to be applied to the velocity |
Reimplemented from SurgSim::Physics::Representation.
|
overridevirtual |
Preprocessing done before the update call This needs to be called from the outside usually from a Computation.
dt | The time step (in seconds) |
Reimplemented from SurgSim::Physics::Representation.
const Eigen::Matrix< double, 6, 6, Eigen::RowMajor > & SurgSim::Physics::RigidRepresentation::getComplianceMatrix | ( | ) | const |
Retrieve the rigid body 6x6 compliance matrix.
const SurgSim::Math::Matrix66d & SurgSim::Physics::RigidRepresentation::getExternalGeneralizedDamping | ( | ) | const |
SurgSim::DataStructures::BufferedValue< SurgSim::Math::Vector6d > & SurgSim::Physics::RigidRepresentation::getExternalGeneralizedForce | ( | ) |
const SurgSim::Math::Matrix66d & SurgSim::Physics::RigidRepresentation::getExternalGeneralizedStiffness | ( | ) | const |
void SurgSim::Physics::RigidRepresentation::setAngularVelocity | ( | const SurgSim::Math::Vector3d & | angularVelocity | ) |
Set the current angular velocity of the rigid representation.
angularVelocity | The angular velocity |
void SurgSim::Physics::RigidRepresentation::setLinearVelocity | ( | const SurgSim::Math::Vector3d & | linearVelocity | ) |
Set the current linear velocity of the rigid representation.
linearVelocity | The linear velocity |
|
overridevirtual |
Update the representation state to the current time step.
dt | The time step (in seconds) |
Reimplemented from SurgSim::Physics::Representation.