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))