|
| RigidRepresentation (const std::string &name) |
| Constructor. More...
|
|
virtual | ~RigidRepresentation () |
| Destructor. More...
|
|
| 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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
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. More...
|
|
bool | isActive () const |
|
virtual void | setLocalActive (bool val) |
| Set the component's active state. More...
|
|
bool | isLocalActive () const |
|
| Accessible () |
| Default Constructor. More...
|
|
| ~Accessible () |
| Destructor. More...
|
|
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...
|
|
template<> |
boost::any | getValue (const std::string &name) const |
|
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
Add an external generalized force applied to the rigid representation (anywhere).
- Note
- This force is generalized (i.e. it's a 6D vector, containing both 3D force and 3D torque).
-
The stiffness and damping are 6x6 matrices with coupling between the translational and rotation dof.
-
All external generalized forces will be zeroed every afterUpdate call of the rigid representation.
- Parameters
-
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) \]