def makeRobotProblemAndViewerFactory(clients,
rolling_table=True,
rosParam=None):
if rosParam is not None:
import rospy, os, hpp
HumanoidRobot.urdfString = rospy.get_param(rosParam)
srdfFilename = hpp.retrieveRosResource(HumanoidRobot.srdfFilename)
with open(srdfFilename, 'r') as f:
HumanoidRobot.srdfString = f.read()
objects = list()
robot = HumanoidRobot("talos",
"talos",
rootJointType="freeflyer",
client=clients)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-2, 2, -2, 2, 0, 2])
shrinkJointRange(robot, 0.95)
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
ps.selectPathValidation('Graph-Progressive', 0.01)
vf = ViewerFactory(ps)
objects.append(Box(name="box", vf=vf))
robot.setJointBounds("box/root_joint", [-2, 2, -2, 2, 0, 2])
# Loaded as an object to get the visual tags at the right position.
# vf.loadEnvironmentModel (Table, 'table')
if rolling_table:
table = RollingTable(name="table", vf=vf)
else:
table = Table(name="table", vf=vf)
robot.setJointBounds("table/root_joint", [-2, 2, -2, 2, -2, 2])
return robot, ps, vf, table, objects
q_goal_monotone = q_init[:]
q_goal_monotone[rank:rank+3] = [ -0.545, 0.268, 0.7460100959532432]
rank = robot.rankInConfiguration ['box2/base_joint_xyz']
q_goal_monotone[rank:rank+3] = [ -0.445, 0.268, 0.7460100959532432]
q_goal_inverted=q_init[:]
b1 = q_goal_inverted[rankB1:rankB1+7]
q_goal_inverted[rankB1:rankB1+7] = q_goal_inverted[rankB2:rankB2+7]
q_goal_inverted[rankB2:rankB2+7] = b1
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (20)
ps.selectPathValidation ('Discretized', 0.05)
ps.selectPathProjector ('Progressive', 0.1)
# ps.selectPathProjector ('Global', 0.2)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
# Create passive DOF lists {{{4
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['baxter'] = list ()
jointNames['baxterRightSide'] = list ()
jointNames['baxterLeftSide'] = list ()
jointNames['box1'] = list ()
jointNames['box2'] = list ()
except:
print("Did not find viewer")
joint_bounds = {}
def setRobotJointBounds(which):
for jn, bound in joint_bounds[which]:
robot.setJointBounds(jn, bound)
joint_bounds["default"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
shrinkJointRange(robot, 0.6)
joint_bounds["grasp-generation"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
setRobotJointBounds("default")
shrinkJointRange(robot, 0.95)
joint_bounds["planning"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ]
ps.selectPathValidation("Graph-Discretized", 0.05)
#ps.selectPathValidation("Graph-Dichotomy", 0)
#ps.selectPathValidation("Graph-Progressive", 0.02)
ps.selectPathProjector("Progressive", 0.2)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
if isSimulation:
ps.setMaxIterProjection (1)
ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05)
q0 = robot.getCurrentConfig()
vf.loadObstacleModel("package://gerard_bauzil/urdf/gerard_bauzil.urdf", "room")
#vf.loadObstacleModel ("package://agimus_demos/urdf/P72-table.urdf", "table")
# Display Tiago Field of view.
#vf.guiRequest.append( (tiago_fov.loadInGui, {'self':None}))
# Display visibility cones.
vf.addCallback(tiago_fov_gui)
try:
v = vf.createViewer()
except:
print("Did not find viewer")
shrinkJointRange(robot, 0.95)
ps.selectPathValidation("Graph-Dichotomy", 0)
ps.selectPathProjector("Progressive", 0.2)
ps.addPathOptimizer("EnforceTransitionSemantic")
ps.addPathOptimizer("SimpleTimeParameterization")
if isSimulation:
ps.setMaxIterProjection(1)
ps.setParameter("SimpleTimeParameterization/safety", 0.25)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05)
q0 = robot.getCurrentConfig()
q0[:4] = [0, -0.9, 0, 1]
#q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15
cg = ConstraintGraph.buildGenericGraph(robot, "graph", grippers, [
placard.name,
], handlesPerObjects, [
[],
], [], rules)
# cg = ConstraintGraph (robot, "graph")
# factory = ConstraintGraphFactory (cg)
# factory.setGrippers (grippers)
# factory.setObjects ([placard.name,], handlesPerObjects, [[],])
# factory.setRules (rules)
# factory.generate ()
cg.addConstraints(graph=True, constraints=commonConstraints)
ps.selectPathProjector("Progressive", .05)
ps.selectPathValidation("Progressive", 0.025)
cg.initialize()
# Define initial and final configurations
q_goal = [
-0.003429678026293006, 7.761615492429529e-05, 0.8333148411182841,
-0.08000440760954532, 0.06905332841243099, -0.09070086400314036,
0.9902546570793265, 0.2097693637044623, 0.19739743868699455,
-0.6079135018296973, 0.8508704420155889, -0.39897628829947995,
-0.05274298289004072, 0.20772797293264825, 0.1846394290733244,
-0.49824886682709824, 0.5042013065348324, -0.16158420369261683,
-0.039828502509861335, -0.3827070014985058, -0.24118425356319423,
1.0157846623463191, 0.5637424355124602, -1.3378817283780955,
-1.3151786907256797, -0.392409481224193, 0.11332560818107676, 1.06, 1.06,
1.06, 1.06, 1.06, 1.06, 1.0, 1.06, 1.06, -1.06, 1.06, 1.06,
0.35936687035487364, -0.32595302056157444, -0.33115291290191723,
