Simox
2.3.74.0
|
Data Structures | |
struct | JointLimits |
Public Types | |
enum | RobotNodeType { Generic, Joint, Body, Transform } |
Experimental: Additional information about the node. More... | |
Public Types inherited from VirtualRobot::SceneObject | |
enum | VisualizationType { Full, Collision, CollisionData } |
Public Member Functions | |
RobotNode (RobotWeakPtr rob, const std::string &name, float jointLimitLo, float jointLimitHi, VisualizationNodePtr visualization=nullptr, CollisionModelPtr collisionModel=nullptr, float jointValueOffset=0.0f, const SceneObject::Physics &physics={}, CollisionCheckerPtr colChecker=nullptr, RobotNodeType type=Generic) | |
~RobotNode () override | |
RobotPtr | getRobot () const |
void | setJointValue (float q) |
void | collectAllRobotNodes (std::vector< RobotNodePtr > &storeNodes) |
virtual float | getJointValue () const |
void | respectJointLimits (float &jointValue) const |
bool | checkJointLimits (float jointValue, bool verbose=false) const |
virtual Eigen::Matrix4f | getLocalTransformation () |
template<class T > | |
auto | getChildren () |
bool | initialize (SceneObjectPtr parent=SceneObjectPtr(), const std::vector< SceneObjectPtr > &children=std::vector< SceneObjectPtr >()) override |
void | setGlobalPose (const Eigen::Matrix4f &pose) override |
Eigen::Matrix4f | getGlobalPose () const override |
virtual Eigen::Matrix4f | getPoseInRootFrame () const |
virtual Eigen::Matrix4f | getPoseInFrame (const RobotNodePtr &frame) const |
virtual Eigen::Vector3f | getPositionInRootFrame () const |
virtual Eigen::Vector3f | getPositionInFrame (const RobotNodePtr &frame) const |
virtual Eigen::Matrix3f | getOrientationInRootFrame () const |
virtual Eigen::Matrix4f | getPoseInRootFrame (const Eigen::Matrix4f &localPose) const |
virtual Eigen::Vector3f | getPositionInRootFrame (const Eigen::Vector3f &localPosition) const |
virtual Eigen::Vector3f | getDirectionInRootFrame (const Eigen::Vector3f &localPosition) const |
virtual Eigen::Matrix3f | getOrientationInRootFrame (const Eigen::Matrix3f &localOrientation) const |
void | showCoordinateSystem (bool enable, float scaling=1.0f, std::string *text=NULL, const std::string &visualizationType="") override |
void | print (bool printChildren=false, bool printDecoration=true) const override |
float | getJointLimitLo () |
float | getJointLimitHi () |
JointLimits | getJointLimits () |
virtual void | setJointLimits (float lo, float hi) |
bool | isJoint () const |
virtual bool | isTranslationalJoint () const |
virtual bool | isRotationalJoint () const |
virtual bool | isHemisphereJoint () const |
virtual void | setLimitless (bool limitless) |
bool | isLimitless () const |
virtual float | getDelta (float target) |
virtual void | showStructure (bool enable, const std::string &visualizationType="") |
virtual std::vector< RobotNodePtr > | getAllParents (RobotNodeSetPtr rns=RobotNodeSetPtr()) |
virtual RobotNodePtr | clone (RobotPtr newRobot, bool cloneChildren=true, RobotNodePtr initializeWithParent=RobotNodePtr(), CollisionCheckerPtr colChecker=CollisionCheckerPtr(), float scaling=1.0f, bool preventCloningMeshesIfScalingIs1=false) |
float | getJointValueOffset () const |
float | getJointLimitHigh () const |
float | getJointLimitLow () const |
float | scaleJointValue (float val, float min=-1, float max=1) |
float | unscaleJointValue (float val, float min=-1, float max=1) |
float | getScaledJointValue (float min=-1, float max=1) |
void | setScaledJointValue (float val, float min=-1, float max=1) |
void | setScaledJointValueNoUpdate (float val, float min=-1, float max=1) |
virtual void | setMaxVelocity (float maxVel) |
virtual void | setMaxAcceleration (float maxAcc) |
virtual void | setMaxTorque (float maxTo) |
float | getMaxVelocity () |
float | getMaxAcceleration () |
float | getMaxTorque () |
RobotNodeType | getType () |
SceneObjectPtr | clone (const std::string &, CollisionCheckerPtr=CollisionCheckerPtr(), float=1.0f) const |
Forbid cloning method from SceneObject. We need to know the new robot for cloning. More... | |
const DHParameter & | getOptionalDHParameters () |
void | updatePose (bool updateChildren=true) override |
void | copyPoseFrom (const SceneObjectPtr &sceneobj) override |
void | copyPoseFrom (const RobotNodePtr &other) |
virtual void | propagateJointValue (const std::string &jointName, float factor=1.0f) |
virtual std::string | toXML (const std::string &basePath, const std::string &modelPathRelative="models", bool storeSensors=true, bool storeModelFiles=true) |
void | setLocalTransformation (Eigen::Matrix4f &newLocalTransformation) |
virtual void | setJointValueNoUpdate (float q) |
bool | getEnforceJointLimits () const |
void | setEnforceJointLimits (bool value) |
Public Member Functions inherited from VirtualRobot::GraspableSensorizedObject | |
GraspableSensorizedObject (const std::string &name, VisualizationNodePtr visualization=VisualizationNodePtr(), CollisionModelPtr collisionModel=CollisionModelPtr(), const Physics &p=Physics(), CollisionCheckerPtr colChecker=CollisionCheckerPtr()) | |
bool | initialize (SceneObjectPtr parent=SceneObjectPtr(), const std::vector< SceneObjectPtr > &children=std::vector< SceneObjectPtr >()) override |
bool | hasGraspSet (GraspSetPtr graspSet) |
bool | hasGraspSet (const std::string &robotType, const std::string &eef) |
void | addGraspSet (GraspSetPtr graspSet) |
void | includeGraspSet (GraspSetPtr graspSet) |
includeGraspSet More... | |
GraspSetPtr | getGraspSet (EndEffectorPtr eef) |
GraspSetPtr | getGraspSet (const std::string &robotType, const std::string &eefName) |
GraspSetPtr | getGraspSet (const std::string &name) |
const std::vector< GraspSetPtr > & | getAllGraspSets () |
virtual SensorPtr | getSensor (const std::string &name) const |
virtual bool | hasSensor (const std::string &name) const |
virtual std::vector< SensorPtr > | getSensors () const |
virtual bool | registerSensor (SensorPtr sensor) |
void | appendGraspSetsTo (GraspableSensorizedObjectPtr other) const |
void | appendSensorsTo (GraspableSensorizedObjectPtr other) const |
void | printGrasps () const |
Public Member Functions inherited from VirtualRobot::SceneObject | |
SceneObject (const std::string &name, VisualizationNodePtr visualization=VisualizationNodePtr(), CollisionModelPtr collisionModel=CollisionModelPtr(), const Physics &p=Physics(), CollisionCheckerPtr colChecker=CollisionCheckerPtr()) | |
virtual | ~SceneObject () |
std::string | getName () const |
void | setName (const std::string &name) |
virtual Eigen::Vector3f | getGlobalPosition () const |
virtual Eigen::Matrix3f | getGlobalOrientation () const |
virtual Eigen::Matrix4f | getGlobalPose (const Eigen::Matrix4f &localPose) const |
virtual Eigen::Vector3f | getGlobalPosition (const Eigen::Vector3f &localPosition) const |
virtual Eigen::Vector3f | getGlobalDirection (const Eigen::Vector3f &localDircetion) const |
virtual Eigen::Matrix3f | getGlobalOrientation (const Eigen::Matrix3f &localOrientation) const |
virtual void | setGlobalPoseNoChecks (const Eigen::Matrix4f &pose) |
virtual CollisionModelPtr | getCollisionModel () |
virtual CollisionCheckerPtr | getCollisionChecker () |
void | setVisualization (VisualizationNodePtr visualization) |
void | setCollisionModel (CollisionModelPtr colModel) |
virtual VisualizationNodePtr | getVisualization (SceneObject::VisualizationType visuType=SceneObject::Full) |
virtual void | setUpdateVisualization (bool enable) |
bool | getUpdateVisualizationStatus () |
virtual void | setUpdateCollisionModel (bool enable) |
bool | getUpdateCollisionModelStatus () |
virtual void | setupVisualization (bool showVisualization, bool showAttachedVisualizations) |
virtual void | showPhysicsInformation (bool enableCoM, bool enableInertial, VisualizationNodePtr comModel=VisualizationNodePtr()) |
virtual bool | showCoordinateSystemState () |
virtual void | showBoundingBox (bool enable, bool wireframe=false) |
virtual bool | ensureVisualization (const std::string &visualizationType="") |
Eigen::Matrix4f | toLocalCoordinateSystem (const Eigen::Matrix4f &poseGlobal) const |
Eigen::Vector3f | toLocalCoordinateSystemVec (const Eigen::Vector3f &positionGlobal) const |
Eigen::Matrix4f | toGlobalCoordinateSystem (const Eigen::Matrix4f &poseLocal) const |
Eigen::Vector3f | toGlobalCoordinateSystemVec (const Eigen::Vector3f &positionLocal) const |
Eigen::Matrix4f | getTransformationTo (const SceneObjectPtr otherObject) const |
Eigen::Matrix4f | getTransformationFrom (const SceneObjectPtr otherObject) const |
Eigen::Matrix4f | transformTo (const SceneObjectPtr otherObject, const Eigen::Matrix4f &poseInOtherCoordSystem) |
Eigen::Vector3f | transformTo (const SceneObjectPtr otherObject, const Eigen::Vector3f &positionInOtherCoordSystem) |
virtual int | getNumFaces (bool collisionModel=false) |
virtual Eigen::Vector3f | getCoMLocal () |
void | setCoMLocal (const Eigen::Vector3f &comLocal) |
Set a new position for the CoM of this bode. More... | |
virtual Eigen::Vector3f | getCoMGlobal () |
float | getMass () const |
void | setMass (float m) |
Physics::SimulationType | getSimulationType () const |
void | setSimulationType (Physics::SimulationType s) |
Eigen::Matrix3f | getInertiaMatrix () |
Eigen::Matrix3f | getInertiaMatrix (const Eigen::Vector3f &shift) |
If the Inertia Matrix is given at the CoM, this function returns the Inertia Matrix at the parallel shifted coordinate system. The shift is done using the parallel axis theorem (https://en.wikipedia.org/wiki/Parallel_axis_theorem) More... | |
Eigen::Matrix3f | getInertiaMatrix (const Eigen::Vector3f &shift, const Eigen::Matrix3f &rotation) |
Eigen::Matrix3f | getInertiaMatrix (const Eigen::Matrix4f &transform) |
void | setInertiaMatrix (const Eigen::Matrix3f &im) |
float | getFriction () |
void | setFriction (float friction) |
SceneObject::Physics | getPhysics () |
std::vector< std::string > | getIgnoredCollisionModels () |
template<typename T > | |
std::shared_ptr< T > | getVisualization (SceneObject::VisualizationType visuType=SceneObject::Full) |
void | highlight (VisualizationPtr visualization, bool enable) |
SceneObjectPtr | clone (const std::string &name, CollisionCheckerPtr colChecker=CollisionCheckerPtr(), float scaling=1.0f) const |
SceneObjectPtr | clone (CollisionCheckerPtr colChecker=CollisionCheckerPtr(), float scaling=1.0f) const |
virtual bool | attachChild (SceneObjectPtr child) |
virtual void | detachChild (SceneObjectPtr child) |
virtual bool | hasChild (SceneObjectPtr child, bool recursive=false) const |
virtual bool | hasChild (const std::string &childName, bool recursive=false) const |
virtual bool | hasParent () const |
virtual SceneObjectPtr | getParent () const |
virtual std::vector< SceneObjectPtr > | getChildren () const |
virtual bool | saveModelFiles (const std::string &modelPath, bool replaceFilenames) |
void | setScaling (float scaling) |
float | getScaling () |
bool | reloadVisualizationFromXML (bool useVisAsColModelIfMissing=true) |
const std::string & | getVisualizationModelXML () const |
Protected Member Functions | |
virtual void | updateTransformationMatrices () |
void | updatePose (const Eigen::Matrix4f &parentPose, bool updateChildren=true) override |
virtual void | updateVisualizationPose (const Eigen::Matrix4f &globalPose, bool updateChildren=false) |
virtual void | updateVisualizationPose (const Eigen::Matrix4f &globalPose, float jointValue, bool updateChildren=false) |
virtual void | checkValidRobotNodeType () |
Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization. More... | |
RobotNode () | |
virtual void | updateTransformationMatrices (const Eigen::Matrix4f &parentPose) |
virtual RobotNodePtr | _clone (const RobotPtr newRobot, const VisualizationNodePtr visualizationModel, const CollisionModelPtr collisionModel, CollisionCheckerPtr colChecker, float scaling)=0 |
SceneObject * | _clone (const std::string &, CollisionCheckerPtr=CollisionCheckerPtr(), float=1.0f) const override |
virtual std::string | _toXML (const std::string &modelPath)=0 |
void | setJointValueNotInitialized (float q) |
Protected Member Functions inherited from VirtualRobot::GraspableSensorizedObject | |
GraspableSensorizedObject () | |
std::string | getGraspableSensorizedObjectXML (const std::string &modelPathRelative="models", bool storeSensors=true, int tabs=0) |
Protected Member Functions inherited from VirtualRobot::SceneObject | |
virtual void | detachedFromParent () |
Parent detached this object. More... | |
virtual void | attached (SceneObjectPtr parent) |
Parent attached this object. More... | |
std::string | getFilenameReplacementVisuModel (const std::string standardExtension=".wrl") |
std::string | getFilenameReplacementColModel (const std::string standardExtension=".wrl") |
SceneObject () | |
std::string | getSceneObjectXMLString (const std::string &basePath, int tabs, const std::string &modelPathRelative="") |
basic data, used by Obstacle and ManipulationObject More... | |
virtual bool | initializePhysics () |
Friends | |
class | Robot |
class | LocalRobot |
class | RobotIO |
class | RobotNodeSet |
class | RobotConfig |
class | RobotFactory |
class | RobotNodeActuator |
class | ColladaIO |
Additional Inherited Members | |
Static Public Member Functions inherited from VirtualRobot::SceneObject | |
static Eigen::Matrix3f | shiftInertia (const Eigen::Matrix3f inertiaMatrix, const Eigen::Vector3f &shift, float mass) |
The shift is done using the parallel axis theorem (https://en.wikipedia.org/wiki/Parallel_axis_theorem) More... | |
Each RobotNode may consider two transformations: localTransformation: This transformation is fixed. jointTransformation: This is the flexible part of the joint. Here, the jointValue is used to compute the transformation according to the implementation of the joint type.
The visualization (of limb and/or coordinateAxis) is linked to the local coordinate sysytem of this joint: localTransformation*jointTransformation The global pose of this joint is the same: localTransformation*jointTransformation
Experimental: Additional information about the node.
VirtualRobot::RobotNode::RobotNode | ( | RobotWeakPtr | rob, |
const std::string & | name, | ||
float | jointLimitLo, | ||
float | jointLimitHi, | ||
VisualizationNodePtr | visualization = nullptr , |
||
CollisionModelPtr | collisionModel = nullptr , |
||
float | jointValueOffset = 0.0f , |
||
const SceneObject::Physics & | physics = {} , |
||
CollisionCheckerPtr | colChecker = nullptr , |
||
RobotNodeType | type = Generic |
||
) |
Constructor with settings.
|
override |
|
inlineprotected |
|
protectedpure virtual |
Derived classes must implement their clone method here. Passed models are already scaled. Scaling factor should be only used for kinematic computations.
Implemented in VirtualRobot::RobotNodeHemisphere, VirtualRobot::RobotNodeRevolute, VirtualRobot::RobotNodePrismatic, and VirtualRobot::RobotNodeFixed.
|
inlineoverrideprotectedvirtual |
Reimplemented from VirtualRobot::SceneObject.
|
protectedpure virtual |
Derived classes add custom XML tags here
Implemented in VirtualRobot::RobotNodeHemisphere, VirtualRobot::RobotNodePrismatic, VirtualRobot::RobotNodeRevolute, and VirtualRobot::RobotNodeFixed.
bool VirtualRobot::RobotNode::checkJointLimits | ( | float | jointValue, |
bool | verbose = false |
||
) | const |
Checks if jointValue is within joint limits. If verbose is set a warning is printed.
|
protectedvirtual |
Checks if nodeType constraints are fulfilled. Otherwise an exception is thrown. Called on initialization.
Reimplemented in VirtualRobot::RobotNodeHemisphere, VirtualRobot::RobotNodeRevolute, VirtualRobot::RobotNodePrismatic, and VirtualRobot::RobotNodeFixed.
|
virtual |
Clone this RobotNode.
newRobot | The newly created RobotNode belongs to newRobot. |
cloneChildren | If true, all children are cloned (and their children, etc). |
initializeWithParent | If given, the RobotNode is initialized with this parent. |
colChecker | Must only be set if the cloned RobotNode should be registered to a different collision checker instance. |
scaling | Scale Can be set to create a scaled version of this robot. Scaling is applied on kinematic, visual, and collision data. |
|
inline |
Forbid cloning method from SceneObject. We need to know the new robot for cloning.
void VirtualRobot::RobotNode::collectAllRobotNodes | ( | std::vector< RobotNodePtr > & | storeNodes | ) |
All children and their children (and so on) are collected. The current instance is also added.
|
overridevirtual |
Reimplemented from VirtualRobot::SceneObject.
void VirtualRobot::RobotNode::copyPoseFrom | ( | const RobotNodePtr & | other | ) |
|
virtual |
Find all robot nodes whose movements affect this RobotNode
rns | If set, the search is limited to the rns. |
|
inline |
|
virtual |
target | the target joint value in [rad] |
|
virtual |
bool VirtualRobot::RobotNode::getEnforceJointLimits | ( | ) | const |
|
overridevirtual |
The global pose defines the position of the object in the world. For non-joint objects it is identical with the visualization frame.
Reimplemented from VirtualRobot::SceneObject.
float VirtualRobot::RobotNode::getJointLimitHi | ( | ) |
|
inline |
float VirtualRobot::RobotNode::getJointLimitLo | ( | ) |
|
inline |
|
inline |
|
virtual |
|
inline |
|
inlinevirtual |
The preJoint/preVisualization transformation. This transformation is applied before the joint and the visualization.
float VirtualRobot::RobotNode::getMaxAcceleration | ( | ) |
Maximum acceleration in rad/s^2 or m/s^2. Returns -1.0f if no maxium value is set.
float VirtualRobot::RobotNode::getMaxTorque | ( | ) |
Maximum acceleration in Nm. Returns -1.0f if no maxium value is set.
float VirtualRobot::RobotNode::getMaxVelocity | ( | ) |
Maximum velocity in rad/s or m/s. Returns -1.0f if no maxium value is set.
|
inline |
When joint was created via DH parameters, they can be accessed here.
|
virtual |
|
virtual |
|
virtual |
|
virtual |
The pose of this node in the root coordinate system of the robot.
|
virtual |
|
virtual |
|
virtual |
The position of this node in the root coordinate system of the robot.
|
virtual |
RobotPtr VirtualRobot::RobotNode::getRobot | ( | ) | const |
|
inline |
RobotNode::RobotNodeType VirtualRobot::RobotNode::getType | ( | ) |
|
overridevirtual |
Initialize robot node. Here pointers to robot and children are created from names. Be sure all children are created and registered to robot before calling initialize. Usually RobotFactory manages the initialization.
Reimplemented from VirtualRobot::SceneObject.
Reimplemented in VirtualRobot::RobotNodeHemisphere, VirtualRobot::RobotNodePrismatic, VirtualRobot::RobotNodeFixed, and VirtualRobot::RobotNodeRevolute.
|
virtual |
Reimplemented in VirtualRobot::RobotNodeHemisphere.
bool VirtualRobot::RobotNode::isJoint | ( | ) | const |
bool VirtualRobot::RobotNode::isLimitless | ( | ) | const |
|
virtual |
Reimplemented in VirtualRobot::RobotNodeRevolute.
|
virtual |
Reimplemented in VirtualRobot::RobotNodePrismatic.
|
overridevirtual |
Print status information.
Reimplemented from VirtualRobot::SceneObject.
Reimplemented in VirtualRobot::RobotNodeHemisphere, VirtualRobot::RobotNodePrismatic, VirtualRobot::RobotNodeFixed, and VirtualRobot::RobotNodeRevolute.
|
virtual |
Automatically propagate the joint value to another joint.
jointName | The name of the other joint. must be part of the same robot. |
factor | The propagated joint value is scaled according to this value. |
void VirtualRobot::RobotNode::respectJointLimits | ( | float & | jointValue | ) | const |
Checks if jointValue is within joint limits. If not jointValue is adjusted.
|
inline |
void VirtualRobot::RobotNode::setEnforceJointLimits | ( | bool | value | ) |
|
overridevirtual |
Calling this method will cause an exception, since RobotNodes are controlled via joint values.
Reimplemented from VirtualRobot::SceneObject.
|
virtual |
Set joint limits [rad].
void VirtualRobot::RobotNode::setJointValue | ( | float | q | ) |
Set a joint value [rad]. The internal matrices and visualizations are updated accordingly. If you intend to update multiple joints, use setJointValueNoUpdate(float) for faster access.
|
protected |
|
virtual |
Set the joint value without updating the internal matrices. After setting all jointvalues the transformations are calculated by calling applyJointValues() This method is used when multiple joints should be updated at once. Access by RobotNodeSets, Robots or RobotConfigs must be protected by a WriteLock.
q | The joint value [rad] eventually clamped to limits. |
|
virtual |
limitless | wheter this node has joint limits or not. |
void VirtualRobot::RobotNode::setLocalTransformation | ( | Eigen::Matrix4f & | newLocalTransformation | ) |
Set the local transformation matrix that is used in this node.
|
virtual |
Set maximum acceleration pf this joint in rad/s^2 or m/s^2. To disbale max. acceleration set to -1.0f.
|
virtual |
Set maximum torque pf this joint in Nm. To disbale max. torque set to -1.0f.
|
virtual |
Set maximum velocity of this joint in rad/s or m/s. To disbale max. velocity set to -1.0f.
|
inline |
|
inline |
|
overridevirtual |
Display the coordinate system of this RobotNode. This is the global pose of it's visualization. enable
Show or hide coordinate system scaling
Size of coordinate system text
Text to display at coordinate system. If not given, the name of this robot node will be displayed. izationType
This option is only needed when the current robot node does not yet own a visualization. Then a visualziationNode has to be built and the visualizationType
specifies which type of visualization should be used. If not given, the first registered visaulizationfactory is used.
Reimplemented from VirtualRobot::SceneObject.
|
virtual |
Visualize the structure of this RobotNode. enable
Show or hide the structure visualization visualizationType
This option is only needed when the current robot node does not yet own a visualization. Then a visualziationNode has to be build and the visualizationType
specifies which type of visualization should be used. If not given, the first registered visaulizationfactory is used.
|
virtual |
Creates an XML string that defines the robotnode. Filenames of all visualization models are set to modelPath/RobotNodeName_visu and/or modelPath/RobotNodeName_colmodel.
|
inline |
|
overridevirtual |
Compute/Update the transformations of this joint and all child joints. Therefore the parent is queried for its pose. This method is called by the robot in order to update the pose matrices.
Reimplemented from VirtualRobot::SceneObject.
|
overrideprotectedvirtual |
Update the pose according to parent
Reimplemented from VirtualRobot::SceneObject.
|
protectedvirtual |
Queries parent for global pose and updates visualization accordingly
|
protectedvirtual |
|
protectedvirtual |
Can be called by a RobotNodeActuator in order to set the pose of the visualization, which means that the preJointTransform is ignored and the pose of the visualization can be set directly. This is useful, if the node is actuated externally, i.e. via a physics engine. todo: Protect such updates by a mutex.
globalPose | The new global pose. The joint value is not determined from this pose. The RobotNodeActuator is responsible for setting the corresponding joint value |
updateChildren | Usually it is assumed that all RobotNodes are updated this way (updateChildren=false). If not, the children poses can be updated according to this node. |
Reimplemented in VirtualRobot::RobotNodeRevolute, and VirtualRobot::RobotNodePrismatic.
|
protectedvirtual |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
friend |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |
given in Nm
|
protected |
given in m/s
|
protected |
given in m/s^2
|
protected |
|
protected |
|
protected |
|
protected |
|
protected |