Simox  2.3.74.0
VirtualRobot::RobotNode Class Referenceabstract
Inheritance diagram for VirtualRobot::RobotNode:
VirtualRobot::GraspableSensorizedObject VirtualRobot::SceneObject VirtualRobot::RobotNodeFixed VirtualRobot::RobotNodeHemisphere VirtualRobot::RobotNodePrismatic VirtualRobot::RobotNodeRevolute

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< RobotNodePtrgetAllParents (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 DHParametergetOptionalDHParameters ()
 
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< SensorPtrgetSensors () 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< SceneObjectPtrgetChildren () 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 ()
 

Protected Attributes

float jointValueOffset
 
float jointLimitLo
 
float jointLimitHi
 
bool enforceJointLimits = true
 
bool limitless
 
DHParameter optionalDHParameter
 
float maxVelocity
 
float maxAcceleration
 given in m/s More...
 
float maxTorque
 given in m/s^2 More...
 
Eigen::Matrix4f localTransformation
 given in Nm More...
 
std::map< std::string, float > propagatedJointValues
 
RobotWeakPtr robot
 
RobotNodeType nodeType
 
float jointValue
 
- Protected Attributes inherited from VirtualRobot::GraspableSensorizedObject
std::vector< GraspSetPtrgraspSets
 
std::vector< SensorPtrsensors
 
- Protected Attributes inherited from VirtualRobot::SceneObject
std::string name
 
bool initialized
 
Eigen::Matrix4f globalPose
 
std::vector< SceneObjectPtrchildren
 
SceneObjectWeakPtr parent
 
CollisionModelPtr collisionModel
 
std::string collisionModelXML
 
VisualizationNodePtr visualizationModel
 
std::string visualizationModelXML
 
std::filesystem::path basePath
 
bool updateVisualization
 
bool updateCollisionModel
 
Physics physics
 
CollisionCheckerPtr collisionChecker
 
float scaling = 1.0f
 

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...
 

Detailed Description

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

Member Enumeration Documentation

◆ RobotNodeType

Experimental: Additional information about the node.

Enumerator
Generic 
Joint 

No constraints.

Body 

Only pre-joint transformations allowed (e.g. DH-theta for revolute joints). No visualization/collision models.

Transform 

No transformations allowed. Only visualization and/or collision models together with physic information.

Only transformations allowed. No joint or model tags.

Constructor & Destructor Documentation

◆ RobotNode() [1/2]

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.

◆ ~RobotNode()

VirtualRobot::RobotNode::~RobotNode ( )
override

◆ RobotNode() [2/2]

VirtualRobot::RobotNode::RobotNode ( )
inlineprotected

Member Function Documentation

◆ _clone() [1/2]

virtual RobotNodePtr VirtualRobot::RobotNode::_clone ( const RobotPtr  newRobot,
const VisualizationNodePtr  visualizationModel,
const CollisionModelPtr  collisionModel,
CollisionCheckerPtr  colChecker,
float  scaling 
)
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.

◆ _clone() [2/2]

SceneObject* VirtualRobot::RobotNode::_clone ( const std::string &  ,
CollisionCheckerPtr  = CollisionCheckerPtr(),
float  = 1.0f 
) const
inlineoverrideprotectedvirtual

Reimplemented from VirtualRobot::SceneObject.

◆ _toXML()

virtual std::string VirtualRobot::RobotNode::_toXML ( const std::string &  modelPath)
protectedpure virtual

◆ checkJointLimits()

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.

◆ checkValidRobotNodeType()

void VirtualRobot::RobotNode::checkValidRobotNodeType ( )
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.

◆ clone() [1/2]

RobotNodePtr VirtualRobot::RobotNode::clone ( RobotPtr  newRobot,
bool  cloneChildren = true,
RobotNodePtr  initializeWithParent = RobotNodePtr(),
CollisionCheckerPtr  colChecker = CollisionCheckerPtr(),
float  scaling = 1.0f,
bool  preventCloningMeshesIfScalingIs1 = false 
)
virtual

Clone this RobotNode.

Parameters
newRobotThe newly created RobotNode belongs to newRobot.
cloneChildrenIf true, all children are cloned (and their children, etc).
initializeWithParentIf given, the RobotNode is initialized with this parent.
colCheckerMust only be set if the cloned RobotNode should be registered to a different collision checker instance.
scalingScale Can be set to create a scaled version of this robot. Scaling is applied on kinematic, visual, and collision data.

◆ clone() [2/2]

SceneObjectPtr VirtualRobot::RobotNode::clone ( const std::string &  ,
CollisionCheckerPtr  = CollisionCheckerPtr(),
float  = 1.0f 
) const
inline

Forbid cloning method from SceneObject. We need to know the new robot for cloning.

◆ collectAllRobotNodes()

void VirtualRobot::RobotNode::collectAllRobotNodes ( std::vector< RobotNodePtr > &  storeNodes)

All children and their children (and so on) are collected. The current instance is also added.

◆ copyPoseFrom() [1/2]

void VirtualRobot::RobotNode::copyPoseFrom ( const SceneObjectPtr sceneobj)
overridevirtual

Reimplemented from VirtualRobot::SceneObject.

◆ copyPoseFrom() [2/2]

void VirtualRobot::RobotNode::copyPoseFrom ( const RobotNodePtr other)

◆ getAllParents()

std::vector< RobotNodePtr > VirtualRobot::RobotNode::getAllParents ( RobotNodeSetPtr  rns = RobotNodeSetPtr())
virtual

Find all robot nodes whose movements affect this RobotNode

Parameters
rnsIf set, the search is limited to the rns.

◆ getChildren()

template<class T >
auto VirtualRobot::RobotNode::getChildren ( )
inline

◆ getDelta()

float VirtualRobot::RobotNode::getDelta ( float  target)
virtual
Parameters
targetthe target joint value in [rad]
Returns
the signed distance between current and target joint values in [rad]. If the given target value violates joint limits or this robotnode is not a joint, 0.0f is returned instead.

◆ getDirectionInRootFrame()

Eigen::Vector3f VirtualRobot::RobotNode::getDirectionInRootFrame ( const Eigen::Vector3f &  localPosition) const
virtual

◆ getEnforceJointLimits()

bool VirtualRobot::RobotNode::getEnforceJointLimits ( ) const

◆ getGlobalPose()

Eigen::Matrix4f VirtualRobot::RobotNode::getGlobalPose ( ) 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.

◆ getJointLimitHi()

float VirtualRobot::RobotNode::getJointLimitHi ( )

◆ getJointLimitHigh()

float VirtualRobot::RobotNode::getJointLimitHigh ( ) const
inline

◆ getJointLimitLo()

float VirtualRobot::RobotNode::getJointLimitLo ( )

◆ getJointLimitLow()

float VirtualRobot::RobotNode::getJointLimitLow ( ) const
inline

◆ getJointLimits()

JointLimits VirtualRobot::RobotNode::getJointLimits ( )
inline

◆ getJointValue()

float VirtualRobot::RobotNode::getJointValue ( ) const
virtual

◆ getJointValueOffset()

float VirtualRobot::RobotNode::getJointValueOffset ( ) const
inline

◆ getLocalTransformation()

virtual Eigen::Matrix4f VirtualRobot::RobotNode::getLocalTransformation ( )
inlinevirtual

The preJoint/preVisualization transformation. This transformation is applied before the joint and the visualization.

◆ getMaxAcceleration()

float VirtualRobot::RobotNode::getMaxAcceleration ( )

Maximum acceleration in rad/s^2 or m/s^2. Returns -1.0f if no maxium value is set.

◆ getMaxTorque()

float VirtualRobot::RobotNode::getMaxTorque ( )

Maximum acceleration in Nm. Returns -1.0f if no maxium value is set.

◆ getMaxVelocity()

float VirtualRobot::RobotNode::getMaxVelocity ( )

Maximum velocity in rad/s or m/s. Returns -1.0f if no maxium value is set.

◆ getOptionalDHParameters()

const DHParameter& VirtualRobot::RobotNode::getOptionalDHParameters ( )
inline

When joint was created via DH parameters, they can be accessed here.

◆ getOrientationInRootFrame() [1/2]

Eigen::Matrix3f VirtualRobot::RobotNode::getOrientationInRootFrame ( ) const
virtual

◆ getOrientationInRootFrame() [2/2]

Eigen::Matrix3f VirtualRobot::RobotNode::getOrientationInRootFrame ( const Eigen::Matrix3f &  localOrientation) const
virtual

◆ getPoseInFrame()

Eigen::Matrix4f VirtualRobot::RobotNode::getPoseInFrame ( const RobotNodePtr frame) const
virtual

◆ getPoseInRootFrame() [1/2]

Eigen::Matrix4f VirtualRobot::RobotNode::getPoseInRootFrame ( ) const
virtual

The pose of this node in the root coordinate system of the robot.

Returns
The pose in root frame

◆ getPoseInRootFrame() [2/2]

Eigen::Matrix4f VirtualRobot::RobotNode::getPoseInRootFrame ( const Eigen::Matrix4f &  localPose) const
virtual

◆ getPositionInFrame()

Eigen::Vector3f VirtualRobot::RobotNode::getPositionInFrame ( const RobotNodePtr frame) const
virtual

◆ getPositionInRootFrame() [1/2]

Eigen::Vector3f VirtualRobot::RobotNode::getPositionInRootFrame ( ) const
virtual

The position of this node in the root coordinate system of the robot.

Returns
The position in root frame

◆ getPositionInRootFrame() [2/2]

Eigen::Vector3f VirtualRobot::RobotNode::getPositionInRootFrame ( const Eigen::Vector3f &  localPosition) const
virtual

◆ getRobot()

RobotPtr VirtualRobot::RobotNode::getRobot ( ) const

◆ getScaledJointValue()

float VirtualRobot::RobotNode::getScaledJointValue ( float  min = -1,
float  max = 1 
)
inline

◆ getType()

RobotNode::RobotNodeType VirtualRobot::RobotNode::getType ( )

◆ initialize()

bool VirtualRobot::RobotNode::initialize ( SceneObjectPtr  parent = SceneObjectPtr(),
const std::vector< SceneObjectPtr > &  children = std::vector<SceneObjectPtr>() 
)
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.

◆ isHemisphereJoint()

bool VirtualRobot::RobotNode::isHemisphereJoint ( ) const
virtual

◆ isJoint()

bool VirtualRobot::RobotNode::isJoint ( ) const

◆ isLimitless()

bool VirtualRobot::RobotNode::isLimitless ( ) const

◆ isRotationalJoint()

bool VirtualRobot::RobotNode::isRotationalJoint ( ) const
virtual

◆ isTranslationalJoint()

bool VirtualRobot::RobotNode::isTranslationalJoint ( ) const
virtual

◆ print()

void VirtualRobot::RobotNode::print ( bool  printChildren = false,
bool  printDecoration = true 
) const
overridevirtual

◆ propagateJointValue()

void VirtualRobot::RobotNode::propagateJointValue ( const std::string &  jointName,
float  factor = 1.0f 
)
virtual

Automatically propagate the joint value to another joint.

Parameters
jointNameThe name of the other joint. must be part of the same robot.
factorThe propagated joint value is scaled according to this value.

◆ respectJointLimits()

void VirtualRobot::RobotNode::respectJointLimits ( float &  jointValue) const

Checks if jointValue is within joint limits. If not jointValue is adjusted.

◆ scaleJointValue()

float VirtualRobot::RobotNode::scaleJointValue ( float  val,
float  min = -1,
float  max = 1 
)
inline

◆ setEnforceJointLimits()

void VirtualRobot::RobotNode::setEnforceJointLimits ( bool  value)

◆ setGlobalPose()

void VirtualRobot::RobotNode::setGlobalPose ( const Eigen::Matrix4f &  pose)
overridevirtual

Calling this method will cause an exception, since RobotNodes are controlled via joint values.

Reimplemented from VirtualRobot::SceneObject.

◆ setJointLimits()

void VirtualRobot::RobotNode::setJointLimits ( float  lo,
float  hi 
)
virtual

Set joint limits [rad].

◆ setJointValue()

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.

◆ setJointValueNotInitialized()

void VirtualRobot::RobotNode::setJointValueNotInitialized ( float  q)
protected

◆ setJointValueNoUpdate()

void VirtualRobot::RobotNode::setJointValueNoUpdate ( float  q)
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.

Parameters
qThe joint value [rad] eventually clamped to limits.

◆ setLimitless()

void VirtualRobot::RobotNode::setLimitless ( bool  limitless)
virtual
Parameters
limitlesswheter this node has joint limits or not.

◆ setLocalTransformation()

void VirtualRobot::RobotNode::setLocalTransformation ( Eigen::Matrix4f &  newLocalTransformation)

Set the local transformation matrix that is used in this node.

◆ setMaxAcceleration()

void VirtualRobot::RobotNode::setMaxAcceleration ( float  maxAcc)
virtual

Set maximum acceleration pf this joint in rad/s^2 or m/s^2. To disbale max. acceleration set to -1.0f.

◆ setMaxTorque()

void VirtualRobot::RobotNode::setMaxTorque ( float  maxTo)
virtual

Set maximum torque pf this joint in Nm. To disbale max. torque set to -1.0f.

◆ setMaxVelocity()

void VirtualRobot::RobotNode::setMaxVelocity ( float  maxVel)
virtual

Set maximum velocity of this joint in rad/s or m/s. To disbale max. velocity set to -1.0f.

◆ setScaledJointValue()

void VirtualRobot::RobotNode::setScaledJointValue ( float  val,
float  min = -1,
float  max = 1 
)
inline

◆ setScaledJointValueNoUpdate()

void VirtualRobot::RobotNode::setScaledJointValueNoUpdate ( float  val,
float  min = -1,
float  max = 1 
)
inline

◆ showCoordinateSystem()

void VirtualRobot::RobotNode::showCoordinateSystem ( bool  enable,
float  scaling = 1.0f,
std::string *  text = NULL,
const std::string &  visualizationType = "" 
)
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.

◆ showStructure()

void VirtualRobot::RobotNode::showStructure ( bool  enable,
const std::string &  visualizationType = "" 
)
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.

◆ toXML()

std::string VirtualRobot::RobotNode::toXML ( const std::string &  basePath,
const std::string &  modelPathRelative = "models",
bool  storeSensors = true,
bool  storeModelFiles = true 
)
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.

See also
RobotIO::saveXML.

◆ unscaleJointValue()

float VirtualRobot::RobotNode::unscaleJointValue ( float  val,
float  min = -1,
float  max = 1 
)
inline

◆ updatePose() [1/2]

void VirtualRobot::RobotNode::updatePose ( bool  updateChildren = true)
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.

◆ updatePose() [2/2]

void VirtualRobot::RobotNode::updatePose ( const Eigen::Matrix4f &  parentPose,
bool  updateChildren = true 
)
overrideprotectedvirtual

Update the pose according to parent

Reimplemented from VirtualRobot::SceneObject.

◆ updateTransformationMatrices() [1/2]

void VirtualRobot::RobotNode::updateTransformationMatrices ( )
protectedvirtual

Queries parent for global pose and updates visualization accordingly

◆ updateTransformationMatrices() [2/2]

void VirtualRobot::RobotNode::updateTransformationMatrices ( const Eigen::Matrix4f &  parentPose)
protectedvirtual

◆ updateVisualizationPose() [1/2]

void VirtualRobot::RobotNode::updateVisualizationPose ( const Eigen::Matrix4f &  globalPose,
bool  updateChildren = false 
)
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.

Parameters
globalPoseThe new global pose. The joint value is not determined from this pose. The RobotNodeActuator is responsible for setting the corresponding joint value
updateChildrenUsually 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.

◆ updateVisualizationPose() [2/2]

void VirtualRobot::RobotNode::updateVisualizationPose ( const Eigen::Matrix4f &  globalPose,
float  jointValue,
bool  updateChildren = false 
)
protectedvirtual

Friends And Related Function Documentation

◆ ColladaIO

friend class ColladaIO
friend

◆ LocalRobot

friend class LocalRobot
friend

◆ Robot

friend class Robot
friend

◆ RobotConfig

friend class RobotConfig
friend

◆ RobotFactory

friend class RobotFactory
friend

◆ RobotIO

friend class RobotIO
friend

◆ RobotNodeActuator

friend class RobotNodeActuator
friend

◆ RobotNodeSet

friend class RobotNodeSet
friend

Field Documentation

◆ enforceJointLimits

bool VirtualRobot::RobotNode::enforceJointLimits = true
protected

◆ jointLimitHi

float VirtualRobot::RobotNode::jointLimitHi
protected

◆ jointLimitLo

float VirtualRobot::RobotNode::jointLimitLo
protected

◆ jointValue

float VirtualRobot::RobotNode::jointValue
protected

◆ jointValueOffset

float VirtualRobot::RobotNode::jointValueOffset
protected

◆ limitless

bool VirtualRobot::RobotNode::limitless
protected

◆ localTransformation

Eigen::Matrix4f VirtualRobot::RobotNode::localTransformation
protected

given in Nm

◆ maxAcceleration

float VirtualRobot::RobotNode::maxAcceleration
protected

given in m/s

◆ maxTorque

float VirtualRobot::RobotNode::maxTorque
protected

given in m/s^2

◆ maxVelocity

float VirtualRobot::RobotNode::maxVelocity
protected

◆ nodeType

RobotNodeType VirtualRobot::RobotNode::nodeType
protected

◆ optionalDHParameter

DHParameter VirtualRobot::RobotNode::optionalDHParameter
protected

◆ propagatedJointValues

std::map< std::string, float > VirtualRobot::RobotNode::propagatedJointValues
protected

◆ robot

RobotWeakPtr VirtualRobot::RobotNode::robot
protected