from hpp.corbaserver.ur5_robot import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import CORBA robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [0, -1.57, 1.57, 3.267256451, 0, 0] q_init = q1; # q_goal = q2 q_goal = q3 from hpp.gepetto import ViewerFactory, PathPlayerGui vf = ViewerFactory (ps) # vf.loadObstacleModel ("ur_description","obstacles","obstacles") vf.loadObstacleModel ("ur_description","table","table") # vf.loadObstacleModel ("ur_description","wall","wall") vf(q1) ps.lockJoint("elbow_joint", [q1[2]]) ps.selectPathValidation("Progressive", .05) ps.setParameter("SplineGradientBased/alphaInit", CORBA.Any(CORBA.TC_double, 0.3)) ps.setParameter("SplineGradientBased/alwaysStopAtFirst", CORBA.Any(CORBA.TC_boolean, True)) ps.setParameter("SplineGradientBased/linearizeAtEachStep", CORBA.Any(CORBA.TC_boolean, False))
#/usr/bin/env python # Script which goes with ur_description package. # Load 6-DoF arm robot to test methods. # b /local/mcampana/devel/hpp/src/hpp-fcl/src/narrowphase/narrowphase.cpp:317 from hpp.corbaserver.ur5_robot import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver import numpy as np robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [-0.07, -1.3, 1.4, -0.8, 3.14, 0]; q2 = [-0.2, -1.65, -1.6, 0.4, 0.8, 0] r(q1) qcoll = [0.412954,-1.53138,0.55877,-1.09925,3.28763,-0.878376] r.loadObstacleModel ("ur_description","table","table") r.loadObstacleModel ("ur_description","obstacle_sphere","obstacle_sphere") r(qcoll) robot.isConfigValid(qcoll)
#/usr/bin/env python # Script which goes with ur_description package. # Load 6-DoF arm robot to test methods. from hpp.corbaserver.ur5_robot import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [-0.07, -1.3, 1.4, -0.8, 3.14, 0]; q2 = [-0.2, -1.65, -1.6, 0.4, 0.8, 0] r(q1) robot.isConfigValid(q1); robot.isConfigValid(q2) ps.setInitialConfig (q1); ps.addGoalConfig (q2) r.loadObstacleModel ("ur_description","table","table") r.loadObstacleModel ("ur_description","obstacle_sphere","obstacle_sphere") r(q1) #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm')
#/usr/bin/env python # Script which goes with ur_description package. # Load 6-DoF arm robot to test methods. from hpp.corbaserver.ur5_robot import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2) from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("ur_description","obstacles","obstacles") r.loadObstacleModel ("ur_description","table","table") r.loadObstacleModel ("ur_description","wall","wall") r(q1) #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm') #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT1.rdm') #ps.selectPathPlanner ("VisibilityPrmPlanner") #ps.selectPathValidation ("Dichotomy", 0.)
#/usr/bin/env python # Script which goes with ur_description package. # Load 6-DoF arm robot to test methods. from hpp.corbaserver.ur5_robot import Robot from hpp.corbaserver import Client from hpp.corbaserver import ProblemSolver robot = Robot ('ur5') cl = robot.client ps = ProblemSolver (robot) # q = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'] 6 DoF# q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2) from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) r.loadObstacleModel ("ur_description","obstacles","obstacles") r.loadObstacleModel ("ur_description","table","table") r.loadObstacleModel ("ur_description","wall","wall") r(q1) #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT.rdm') ps.selectPathPlanner ("VisibilityPrmPlanner") #ps.selectPathValidation ("Dichotomy", 0.) ps.solve ()