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)
