robot = Robot ('pr2')
robot.setJointBounds ("root_joint", [-4, -3, -5, -3])
Import class pr2.robot.Robot and create an instance and set bounds of translation degrees of freedom of the base. Note that the constructor of the instance calls idl method hpp::corbaserver::Robot::loadRobotModel. This triggers the loading of the urdf/srdf model of the PR2 robot in hppcorbaserver
executable.
Import class hpp.corbaserver.problem_solver.ProblemSolver and create an instance. This class is a helper class to define and solve path planning problems.
from
hpp.gepetto
import ViewerFactory
vf = ViewerFactory (ps)
Import class gepetto.viewerFactory.ViewerFactory and create an instance. This object takes as input the ProblemSolver
instance that enables the viewer client to also control hppcorbaserver
executable
q_init = robot.getCurrentConfig ()
q_goal = q_init [::]
q_init [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['torso_lift_joint']
q_init [rank] = 0.2
vf (q_init)
Define initial configuration.
- Note
- Initial configuration is built from configuration of the robot at construction, and by modification of joints retrieved by name. This method is more robust than specifying a hard-coded configuration vector since the ordering of joints in the configuration vector is not unique.
q_goal [0:2] = [-3.2, -4]
rank = robot.rankInConfiguration ['l_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['l_elbow_flex_joint']
q_goal [rank] = -0.5
rank = robot.rankInConfiguration ['r_shoulder_lift_joint']
q_goal [rank] = 0.5
rank = robot.rankInConfiguration ['r_elbow_flex_joint']
q_goal [rank] = -0.5
vf (q_goal)
Define goal configuration.
vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
Load obstacles from urdf file.
- Note
- this method loads the objects defined in the urdf file both in hppcorbaserver and in
gepetto-viewer-server
.
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
Define initial and goal configurations.
ps.addPathOptimizer ("RandomShortcut")
Add a path optimizer (hpp::core::RandomShortcut).
Solve problem and print the results.
v = vf.createViewer()
from
hpp.gepetto
import PathPlayer
pp = PathPlayer (v)
Create the display window. Import and create an instance of PathPlayer. This class samples a path in hppcorbaserver
and displays it in gepetto-viewer-server
.
Display first path, result of RRT.
Display second path after optimization.