Public Member Functions | Public Attributes | List of all members
manipulation.robot.Robot Class Reference

Load and handle a composite robot for manipulation planning. More...

Inheritance diagram for manipulation.robot.Robot:
[legend]
Collaboration diagram for manipulation.robot.Robot:
[legend]

Public Member Functions

def __init__ (self, compositeName=None, robotName=None, rootJointType=None, load=True)
 Constructor. More...
 
def loadModel (self, robotName, rootJointType)
 Virtual function to load the robot model. More...
 
def insertRobotModel (self, robotName, rootJointType, packageName, modelName, urdfSuffix, srdfSuffix)
 Load robot model and insert it in the device. More...
 
def insertRobotSRDFModel (self, robotName, packageName, modelName, srdfSuffix)
 Load a SRDF for the robot. More...
 
def insertHumanoidModel (self, robotName, rootJointType, packageName, modelName, urdfSuffix, srdfSuffix)
 Load humanoid robot model and insert it in the device. More...
 
def loadHumanoidModel (self, robotName, rootJointType, packageName, modelName, urdfSuffix, srdfSuffix)
 
def insertObjectModel (self, objectName, rootJointType, packageName, modelName, urdfSuffix, srdfSuffix)
 Load object model and insert it in the device. More...
 
def loadEnvironmentModel (self, packageName, modelName, urdfSuffix, srdfSuffix, envName)
 Load environment model and store in local map. More...
 
def rebuildRanks (self)
 Rebuild inner variables rankInConfiguration and rankInVelocity. More...
 
Degrees of freedom
def getConfigSize (self)
 Get size of configuration. More...
 
def getNumberDof (self)
 
Joints
def getJointNames (self)
 Get joint names in the same order as in the configuration. More...
 
def getAllJointNames (self)
 Get joint names in the same order as in the configuration. More...
 
def getJointPosition (self, jointName)
 Get joint position. More...
 
def setJointPosition (self, jointName, position)
 Set static position of joint in its parent frame. More...
 
def getJointNumberDof (self, jointName)
 Get joint number degrees of freedom. More...
 
def getJointConfigSize (self, jointName)
 Get joint number config size. More...
 
def setJointBounds (self, jointName, inJointBound)
 set bounds for the joint More...
 
def getJointBounds (self, jointName)
 Get bounds for a joint. More...
 
def getSaturated (self, q)
 Get joints that are saturated for a given configuration. More...
 
def setRootJointPosition (self, robotName, position)
 Set the position of root joint of a robot in world frame. More...
 
def getLinkPosition (self, jointName)
 Get link position in joint frame. More...
 
def getLinkNames (self, jointName)
 Get link name. More...
 
Access to current configuration
def setCurrentConfig (self, q)
 Set current configuration of composite robot. More...
 
def getCurrentConfig (self)
 Get current configuration of composite robot. More...
 
def shootRandomConfig (self)
 Shoot random configuration. More...
 
Bodies
def getJointInnerObjects (self, jointName)
 Get the list of objects attached to a joint. More...
 
def getJointOuterObjects (self, jointName)
 Get list of collision objects tested with the body attached to a joint. More...
 
def getObjectPosition (self, objectName)
 Get position of robot object. More...
 
def removeObstacleFromJoint (self, objectName, jointName, collision=True, distance=False)
 Remove an obstacle from outer objects of a joint body. More...
 
def getGripperPositionInJoint (self, gripperName)
 Return the joint name in which a gripper is and the position relatively to the joint. More...
 
def getHandlePositionInJoint (self, handleName)
 Return the joint name in which a handle is and the position relatively to the joint. More...
 
Collision checking and distance computation
def collisionTest (self)
 Test collision with obstacles and auto-collision. More...
 
def isConfigValid (self, cfg)
 Check the validity of a configuration. More...
 
def distancesToCollision (self)
 Compute distances between bodies and obstacles. More...
 
def getRobotAABB (self)
 See hpp.corbaserver.Robot.getRobotAABB. More...
 
Mass and inertia
def getMass (self)
 Get mass of robot. More...
 
def getCenterOfMass (self)
 Get position of center of mass. More...
 
def getJacobianCenterOfMass (self)
 Get Jacobian of the center of mass. More...
 

Public Attributes

 tf_root
 
 rootJointType
 
 client
 
 name
 
 displayName
 
 load
 
 jointNames
 
 rankInConfiguration
 
 rankInVelocity
 

Detailed Description

Load and handle a composite robot for manipulation planning.

A composite robot is a kinematic chain composed of several sub-kinematic chains rooted at an anchor joint.

Constructor & Destructor Documentation

◆ __init__()

def manipulation.robot.Robot.__init__ (   self,
  compositeName = None,
  robotName = None,
  rootJointType = None,
  load = True 
)

Constructor.

Parameters
robotNamename of the first robot that is loaded now,
rootJointTypetype of root joint among ("freeflyer", "planar", "anchor"),
loadwhether to actually load urdf files. Set to no if you only want to initialize a corba client to an already initialized problem.

Member Function Documentation

◆ collisionTest()

def manipulation.robot.Robot.collisionTest (   self)

Test collision with obstacles and auto-collision.

Check whether current configuration of robot is valid by calling CkwsDevice::collisionTest ().

Returns
whether configuration is valid
Note
Deprecated. Use isConfigValid instead.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ distancesToCollision()

def manipulation.robot.Robot.distancesToCollision (   self)

Compute distances between bodies and obstacles.

Returns
list of distances,
names of the objects belonging to a body
names of the objects tested with inner objects,
closest points on the body,
closest points on the obstacles
Note
outer objects for a body can also be inner objects of another body.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ getAllJointNames()

def manipulation.robot.Robot.getAllJointNames (   self)

◆ getCenterOfMass()

def manipulation.robot.Robot.getCenterOfMass (   self)

◆ getConfigSize()

def manipulation.robot.Robot.getConfigSize (   self)

◆ getCurrentConfig()

def manipulation.robot.Robot.getCurrentConfig (   self)

◆ getGripperPositionInJoint()

def manipulation.robot.Robot.getGripperPositionInJoint (   self,
  gripperName 
)

◆ getHandlePositionInJoint()

def manipulation.robot.Robot.getHandlePositionInJoint (   self,
  handleName 
)

◆ getJacobianCenterOfMass()

def manipulation.robot.Robot.getJacobianCenterOfMass (   self)

◆ getJointBounds()

def manipulation.robot.Robot.getJointBounds (   self,
  jointName 
)

Get bounds for a joint.

Parameters
jointNamename of the joint
Returns
sequence of bounds in order [v0_min,v0_max,v1_min,v1_max,...] where vi_min, vi_max are the bounds of the i-th degree of freedom of the joint if the degree of freedom is bounded, 1, 0 otherwise.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

Referenced by manipulation.robot.Robot.getSaturated().

◆ getJointConfigSize()

def manipulation.robot.Robot.getJointConfigSize (   self,
  jointName 
)

◆ getJointInnerObjects()

def manipulation.robot.Robot.getJointInnerObjects (   self,
  jointName 
)

◆ getJointNames()

def manipulation.robot.Robot.getJointNames (   self)

◆ getJointNumberDof()

def manipulation.robot.Robot.getJointNumberDof (   self,
  jointName 
)

◆ getJointOuterObjects()

def manipulation.robot.Robot.getJointOuterObjects (   self,
  jointName 
)

Get list of collision objects tested with the body attached to a joint.

Parameters
inJointNamename of the joint.
Returns
list of names of CollisionObject

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ getJointPosition()

def manipulation.robot.Robot.getJointPosition (   self,
  jointName 
)

◆ getLinkNames()

def manipulation.robot.Robot.getLinkNames (   self,
  jointName 
)

◆ getLinkPosition()

def manipulation.robot.Robot.getLinkPosition (   self,
  jointName 
)

Get link position in joint frame.

Joints are oriented in a different way as in urdf standard since rotation and uni-dimensional translation joints act around or along their x-axis. This method returns the position of the urdf link in world frame.

Parameters
jointNamename of the joint
Returns
position of the link in world frame.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ getMass()

def manipulation.robot.Robot.getMass (   self)

◆ getNumberDof()

def manipulation.robot.Robot.getNumberDof (   self)

◆ getObjectPosition()

def manipulation.robot.Robot.getObjectPosition (   self,
  objectName 
)

◆ getRobotAABB()

def manipulation.robot.Robot.getRobotAABB (   self)

◆ getSaturated()

def manipulation.robot.Robot.getSaturated (   self,
  q 
)

◆ insertHumanoidModel()

def manipulation.robot.Robot.insertHumanoidModel (   self,
  robotName,
  rootJointType,
  packageName,
  modelName,
  urdfSuffix,
  srdfSuffix 
)

Load humanoid robot model and insert it in the device.

Parameters
robotNamekey of the robot in ProblemSolver object map (see hpp::manipulation::ProblemSolver::addRobot)
rootJointTypetype of root joint among "anchor", "freeflyer", "planar",
packageNameName of the ROS package containing the model,
modelNameName of the package containing the model
urdfSuffixsuffix for urdf file,

The ros url are built as follows:

  • "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
  • "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::robot::Robot.client, hpp::corbaserver::problem_solver::ProblemSolver.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, manipulation.constraint_graph.ConstraintGraph.client, manipulation.robot.Robot.load, manipulation.robot.Robot.loadHumanoidModel(), manipulation.robot.Robot.rebuildRanks(), hpp::corbaserver::robot::Robot.rootJointType, and manipulation.robot.Robot.rootJointType.

Referenced by manipulation.robot.Robot.insertRobotSRDFModel(), manipulation.robot.Robot.loadHumanoidModel(), and manipulation.robot.HumanoidRobot.loadModel().

◆ insertObjectModel()

def manipulation.robot.Robot.insertObjectModel (   self,
  objectName,
  rootJointType,
  packageName,
  modelName,
  urdfSuffix,
  srdfSuffix 
)

Load object model and insert it in the device.

Parameters
robotNamekey of the object in ProblemSolver object map (see hpp::manipulation::ProblemSolver::addRobot)
rootJointTypetype of root joint among "anchor", "freeflyer", "planar",
packageNameName of the ROS package containing the model,
modelNameName of the package containing the model
urdfSuffixsuffix for urdf file,

The ros url are built as follows:

  • "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
  • "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::robot::Robot.client, hpp::corbaserver::problem_solver::ProblemSolver.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, manipulation.constraint_graph.ConstraintGraph.client, manipulation.robot.Robot.load, manipulation.robot.Robot.loadEnvironmentModel(), manipulation.robot.Robot.rebuildRanks(), hpp::corbaserver::robot::Robot.rootJointType, and manipulation.robot.Robot.rootJointType.

Referenced by manipulation.robot.Robot.loadHumanoidModel().

◆ insertRobotModel()

def manipulation.robot.Robot.insertRobotModel (   self,
  robotName,
  rootJointType,
  packageName,
  modelName,
  urdfSuffix,
  srdfSuffix 
)

Load robot model and insert it in the device.

Parameters
robotNamekey of the robot in hpp::manipulation::ProblemSolver object map (see hpp::manipulation::ProblemSolver::addRobot)
rootJointTypetype of root joint among "anchor", "freeflyer", "planar",
packageNameName of the ROS package containing the model,
modelNameName of the package containing the model
urdfSuffixsuffix for urdf file,

The ros url are built as follows:

  • "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
  • "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::robot::Robot.client, hpp::corbaserver::problem_solver::ProblemSolver.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, manipulation.constraint_graph.ConstraintGraph.client, manipulation.robot.Robot.insertRobotSRDFModel(), manipulation.robot.Robot.load, manipulation.robot.Robot.rebuildRanks(), hpp::corbaserver::robot::Robot.rootJointType, and manipulation.robot.Robot.rootJointType.

Referenced by manipulation.robot.Robot.loadModel().

◆ insertRobotSRDFModel()

def manipulation.robot.Robot.insertRobotSRDFModel (   self,
  robotName,
  packageName,
  modelName,
  srdfSuffix 
)

Load a SRDF for the robot.

Several SRDF can thus be loaded for the same robot

Parameters
robotNamekey of the robot in hpp::manipulation::Device object map (see hpp::manipulation::Device)
packageNameName of the ROS package containing the model,
modelNameName of the package containing the model
srdfSuffixsuffix for srdf file,

The ros url are built as follows:

  • "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::robot::Robot.client, hpp::corbaserver::problem_solver::ProblemSolver.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, manipulation.constraint_graph.ConstraintGraph.client, manipulation.robot.Robot.insertHumanoidModel(), and manipulation.robot.Robot.load.

Referenced by manipulation.robot.Robot.insertRobotModel().

◆ isConfigValid()

def manipulation.robot.Robot.isConfigValid (   self,
  cfg 
)

Check the validity of a configuration.

Check whether a configuration of robot is valid.

Parameters
cfga configuration
Returns
whether configuration is valid

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ loadEnvironmentModel()

def manipulation.robot.Robot.loadEnvironmentModel (   self,
  packageName,
  modelName,
  urdfSuffix,
  srdfSuffix,
  envName 
)

Load environment model and store in local map.

Contact surfaces are build from the corresping srdf file. See hpp-manipulation-urdf for more details about contact surface specifications.

Parameters
envNamekey of the object in ProblemSolver object map (see hpp::manipulation::ProblemSolver::addRobot)
packageNameName of the ROS package containing the model,
modelNameName of the package containing the model
urdfSuffixsuffix for urdf file,
srdfSuffixsuffix for srdf file.

The ros url are built as follows:

  • "package://${packageName}/urdf/${modelName}${urdfSuffix}.urdf"
  • "package://${packageName}/srdf/${modelName}${srdfSuffix}.srdf"

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::robot::Robot.client, hpp::corbaserver::problem_solver::ProblemSolver.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, manipulation.constraint_graph.ConstraintGraph.client, manipulation.robot.Robot.load, hpp::corbaserver::robot::Robot.rootJointType, and manipulation.robot.Robot.rootJointType.

Referenced by manipulation.robot.Robot.insertObjectModel().

◆ loadHumanoidModel()

def manipulation.robot.Robot.loadHumanoidModel (   self,
  robotName,
  rootJointType,
  packageName,
  modelName,
  urdfSuffix,
  srdfSuffix 
)

◆ loadModel()

def manipulation.robot.Robot.loadModel (   self,
  robotName,
  rootJointType 
)

◆ rebuildRanks()

def manipulation.robot.Robot.rebuildRanks (   self)

Rebuild inner variables rankInConfiguration and rankInVelocity.

Referenced by manipulation.robot.Robot.insertHumanoidModel(), manipulation.robot.Robot.insertObjectModel(), and manipulation.robot.Robot.insertRobotModel().

◆ removeObstacleFromJoint()

def manipulation.robot.Robot.removeObstacleFromJoint (   self,
  objectName,
  jointName,
  collision = True,
  distance = False 
)

Remove an obstacle from outer objects of a joint body.

Parameters
objectNamename of the object to remove,
jointNamename of the joint owning the body,
collisionwhether collision with object should be computed,
distancewhether distance to object should be computed.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

Referenced by manipulation.robot.Robot.getObjectPosition().

◆ setCurrentConfig()

def manipulation.robot.Robot.setCurrentConfig (   self,
  q 
)

◆ setJointBounds()

def manipulation.robot.Robot.setJointBounds (   self,
  jointName,
  inJointBound 
)

◆ setJointPosition()

def manipulation.robot.Robot.setJointPosition (   self,
  jointName,
  position 
)

◆ setRootJointPosition()

def manipulation.robot.Robot.setRootJointPosition (   self,
  robotName,
  position 
)

Set the position of root joint of a robot in world frame.

Parameters
robotNamekey of the robot in ProblemSolver object map.
positionconstant position of the root joint in world frame in initial configuration.

References hpp::corbaserver::benchmark::Benchmark.client, hpp::corbaserver::problem_solver::ProblemSolver.client, hpp::corbaserver::robot::Robot.client, manipulation.problem_solver.ProblemSolver.client, manipulation.robot.Robot.client, and manipulation.constraint_graph.ConstraintGraph.client.

◆ shootRandomConfig()

def manipulation.robot.Robot.shootRandomConfig (   self)

Member Data Documentation

◆ client

manipulation.robot.Robot.client

Referenced by manipulation.robot.Robot.collisionTest(), manipulation.robot.Robot.distancesToCollision(), manipulation.robot.Robot.getAllJointNames(), manipulation.robot.Robot.getCenterOfMass(), manipulation.robot.Robot.getConfigSize(), manipulation.robot.Robot.getCurrentConfig(), manipulation.robot.Robot.getGripperPositionInJoint(), manipulation.robot.Robot.getHandlePositionInJoint(), manipulation.robot.Robot.getJacobianCenterOfMass(), manipulation.robot.Robot.getJointBounds(), manipulation.robot.Robot.getJointConfigSize(), manipulation.robot.Robot.getJointInnerObjects(), manipulation.robot.Robot.getJointNames(), manipulation.robot.Robot.getJointNumberDof(), manipulation.robot.Robot.getJointOuterObjects(), manipulation.robot.Robot.getJointPosition(), manipulation.robot.Robot.getLinkNames(), manipulation.robot.Robot.getLinkPosition(), manipulation.robot.Robot.getMass(), manipulation.robot.Robot.getNumberDof(), manipulation.robot.Robot.getObjectPosition(), manipulation.robot.Robot.getRobotAABB(), manipulation.robot.Robot.insertHumanoidModel(), manipulation.robot.Robot.insertObjectModel(), manipulation.robot.Robot.insertRobotModel(), manipulation.robot.Robot.insertRobotSRDFModel(), manipulation.robot.Robot.isConfigValid(), manipulation.robot.Robot.loadEnvironmentModel(), manipulation.robot.Robot.loadModel(), manipulation.robot.HumanoidRobot.loadModel(), manipulation.robot.Robot.removeObstacleFromJoint(), manipulation.robot.Robot.setCurrentConfig(), manipulation.robot.Robot.setJointBounds(), manipulation.robot.Robot.setJointPosition(), manipulation.robot.Robot.setRootJointPosition(), and manipulation.robot.Robot.shootRandomConfig().

◆ displayName

manipulation.robot.Robot.displayName

◆ jointNames

manipulation.robot.Robot.jointNames

◆ load

manipulation.robot.Robot.load

◆ name

manipulation.robot.Robot.name

◆ rankInConfiguration

manipulation.robot.Robot.rankInConfiguration

◆ rankInVelocity

manipulation.robot.Robot.rankInVelocity

◆ rootJointType

manipulation.robot.Robot.rootJointType

◆ tf_root

manipulation.robot.Robot.tf_root