from hpp.corbaserver.pr2 import Robot robot = Robot ('pr2') robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3]) from hpp_ros import ScenePublisher r = ScenePublisher (robot) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) 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 r (q_init) 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 r (q_goal) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") ps.setInitialConfig (q_init)
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2', False) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) from gepetto.corbaserver import Client as GuiClient guiClient = GuiClient() from hpp.gepetto import Viewer Viewer.sceneName = '0_scene_hpp_' r = Viewer(ps, guiClient) from hpp.gepetto import PathPlayer pp = PathPlayer(robot.client, r)
from hpp.corbaserver.pr2 import Robot robot = Robot ('pr2') robot.setJointBounds ("root_joint", [-4, -3, -5, -3]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) 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) 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) vf.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen") ps.setInitialConfig (q_init)
from hpp.corbaserver.pr2 import Robot robot = Robot("pr2") robot.setJointBounds("base_joint_xy", [-4, -3, -5, -3]) from hpp_ros import ScenePublisher r = ScenePublisher(robot) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) q_init = [ -3.2, -4, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0,
from hpp.corbaserver.pr2 import Robot robot = Robot('pr2') robot.setJointBounds("base_joint_x", [-4, -3]) robot.setJointBounds("base_joint_y", [-5, -3]) from hpp_ros import ScenePublisher r = ScenePublisher(robot) from hpp.corbaserver import ProblemSolver ps = ProblemSolver(robot) q_init = [ -3.2, -4, 0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] r(q_init) q_goal = [ -3.2, -4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0, 0.0, 0.0, 0.0, 0.5, 0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0, -0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] r(q_goal) ps.loadObstacleFromUrdf("iai_maps", "kitchen_area", "") ps.selectPathOptimizer("None") ps.selectPathValidation("Dichotomy", 0)
from hpp.corbaserver.pr2 import Robot from hpp.gepetto import PathPlayer from math import pi robot = Robot ('pr2') robot.setJointBounds ("root_joint", [-4, -3, -5, -3,-2,2,-2,2]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) 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 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.loadObstacleModel ("iai_maps", "kitchen_area", "kitchen")
from hpp.corbaserver.pr2 import Robot from hpp.gepetto import ViewerFactory from hpp.corbaserver import ProblemSolver robot = Robot("pr2") robot.setJointBounds("root_joint", [-4, -3, -5, -3]) ps = ProblemSolver(robot) vf = ViewerFactory(ps) 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) 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) vf.loadObstacleModel("iai_maps", "kitchen_area", "kitchen")
#/usr/bin/env python import time from hpp.corbaserver import ProblemSolver from hpp.corbaserver.pr2 import Robot robot = Robot ('pr2') #35 DOF robot.setJointBounds ("base_joint_xy", [-4, -3, -5, -3]) ps = ProblemSolver (robot) from hpp.gepetto import Viewer, PathPlayer Viewer.withFloor = True r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("iai_maps","kitchen_area","kitchen_area") # visual kitchen 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 r (q_init) 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