cl.robot.setCurrentConfig(q1) cl.robot.collisionTest() res=cl.robot.distancesToCollision() from numpy import * res[1][argmin(res[0])] res[2][argmin(res[0])] r( cl.problem.configAtDistance(0,5) ) cl.problem.optimizePath (0) cl.problem.clearRoadmap () cl.problem.resetGoalConfigs () robot.getJointNames () robot.getConfigSize () cl.obstacle.getObstaclePosition('decor_base') robot.getJointOuterObjects('shoulder_pan_joint') r([-0.4, -2, -1.8, 0, 0.8, 0]) robot.isConfigValid([-0.4, -2, -1.8, 0, 0.8, 0]) # collision not detected with complete mesh ## Debug Optimization Tools ############## num_log = 32174 from parseLog import parseCollConstrPoints, parseNodes collConstrNodes = parseNodes (num_log, '189: qFree_ = ') collNodes = parseNodes (num_log, '182: qColl = ') contactPoints = parseCollConstrPoints (num_log, '77: contact point = (') x1_J1 = parseCollConstrPoints (num_log, '96: x1 in R0 = (') x2_J1 = parseCollConstrPoints (num_log, '97: x2 in R0 = (') x1_J2 = parseCollConstrPoints (num_log, '116: x1 in J2 = (') x2_J2 = parseCollConstrPoints (num_log, '117: x2 in J2 = (') #x2_J2 <=> contactPoints
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) robot.isConfigValid(q1); robot.isConfigValid(q2) ps.setInitialConfig (q1); ps.addGoalConfig (q2) #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm') #ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-RRT.rdm') ps.selectPathPlanner ("VisibilityPrmPlanner") #ps.selectPathValidation ("Dichotomy", 0.) ps.solve () ps.pathLength(0) len(ps.getWaypoints (0))
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') ps.selectPathPlanner ("VisibilityPrmPlanner") #ps.selectPathValidation ("Dichotomy", 0.) ps.solve () ps.pathLength(0)