ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
ps.setErrorThreshold(1e-2)
ps.setMaxIterProjection(40)
robot.setJointBounds('romeo/root_joint', [
-1,
1,
-1,
1,
0,
2,
-2.,
2,
-2.,
2,
-2.,
2,
-2.,
2,
])
placard = Placard('placard', vf)
robot.setJointBounds(placard.name + '/root_joint', [
-1,
1,
-1,
1,
0,
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
srdfSuffix = ""
joint = "cup/base_joint"
handle = "cup/handle"
Robot.srdfSuffix = "_moveit"
# 4}}}
robot = Robot ('romeo-kitchen', 'romeo')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointBounds ("romeo/base_joint_xyz" , [-6,-2,-5.2,-2.7, 0, 2])
vf.loadObjectModel (Kitchen, "kitchen_area")
vf.loadObjectModel (Cup, "cup")
robot.setJointBounds ('cup/base_joint_xyz', [-6,-4,-5,-3,0,1.5])
# 3}}}
# Define configurations. {{{3
robot.setCurrentConfig (robot.getInitialConfig ())
q_init = robot.getHandConfig ("both", "open")
rank = robot.rankInConfiguration ['romeo/base_joint_xyz']
q_init [rank:rank+7] = [-3.5,-3.7, 0.877, 1, 0, 0, 0]
rank = robot.rankInConfiguration ['cup/base_joint_xyz']
q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
q_goal = q_init [::]
q = [None] * N
err = [None] * N
start = time.time()
for i in range(N):
res[i], q[i], err[i] = ps.applyConstraints(qs[i])
duration = time.time() - start
# return res,q,err,duration
return float(res.count(True)) / N,duration / N
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
ps.setErrorThreshold (1e-2)
ps.setMaxIterProjection (40)
robot.setJointBounds ('romeo/root_joint' , [-1,1,-1,1, 0, 2,-2.,2,-2.,2,-2.,2,
-2.,2,])
placard = Placard ('placard', vf)
robot.setJointBounds (placard.name + '/root_joint', [-1,1,-1,1,0,1.5,-2.,2,
-2.,2,-2.,2,-2.,2,])
## Lock both hands
locklhand = list()
for j,v in robot.leftHandOpen.iteritems():
locklhand.append ('romeo/' + j)
if type(v) is float or type(v) is int:
val = [v,]
else:
val = v;
ps.createLockedJoint ('romeo/' + j, robot.displayName + '/' + j, val)
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
srdfSuffix = ""
joint = "cup/base_joint"
handle = "cup/handle"
Robot.srdfSuffix = "_moveit"
# 4}}}
robot = Robot('romeo-kitchen', 'romeo')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
robot.setJointBounds("romeo/base_joint_xyz", [-6, -2, -5.2, -2.7, 0, 2])
vf.loadObjectModel(Kitchen, "kitchen_area")
vf.loadObjectModel(Cup, "cup")
robot.setJointBounds('cup/base_joint_xyz', [-6, -4, -5, -3, 0, 1.5])
# 3}}}
# Define configurations. {{{3
robot.setCurrentConfig(robot.getInitialConfig())
q_init = robot.getHandConfig("both", "open")
rank = robot.rankInConfiguration['romeo/base_joint_xyz']
q_init[rank:rank + 7] = [-3.5, -3.7, 0.877, 1, 0, 0, 0]
rank = robot.rankInConfiguration['cup/base_joint_xyz']
q_init[rank:rank + 7] = [-4.8, -4.6, 0.91, 0, sqrt(2) / 2, sqrt(2) / 2, 0]
q_goal = q_init[::]