Пример #1
0
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))
Пример #2
0
#/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)
Пример #3
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)

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')
Пример #4
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.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-RRT1.rdm')

#ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
Пример #5
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 ()