def makeRobotProblemAndViewerFactory(clients, rolling_table=True):
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")
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
def makeRobotProblemAndViewerFactory(corbaclient):
robot = Robot("dev",
"talos",
rootJointType="freeflyer",
client=corbaclient)
robot.leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver(robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory(ps)
vf.loadObjectModel(Object, "box")
robot.setJointBounds("box/root_joint", [-1, 1, -1, 1, 0, 2])
vf.loadEnvironmentModel(Table, "table")
return robot, ps, vf
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
srdfSuffix = ""
handles = [ "box/top", "box/bottom" ]
robot = Robot ('dev', 'talos', rootJointType = "freeflyer")
robot. leftAnkle = "talos/leg_left_6_joint"
robot.rightAnkle = "talos/leg_right_6_joint"
robot.setJointBounds ("talos/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
ps = ProblemSolver (robot)
ps.setRandomSeed(123)
ps.selectPathProjector("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterProjection (40)
ps.addPathOptimizer("SimpleTimeParameterization")
vf = ViewerFactory (ps)
Object = Box
vf.loadObjectModel (Object, 'box')
robot.setJointBounds ("box/root_joint", [-1, 1, -1, 1, 0, 2])
qq = [-0.7671778026566639, 0.0073267002287253635, 1.0035168727631776, -0.003341673452654457, -0.021566597515109524, 0.0002183620894239602, 0.9997618053357284, -0.00020053128587844821, -0.0021365695604276275, -0.4415951773681094, 0.9659230706255528, -0.48119003672520416, 0.007109157982067145, -0.00020095991543181877, -0.002126639473414498, -0.4382848597339842, 0.9589221865248464, -0.4774994711722908, 0.007099218648561522, 0.0, 0.025235347910697536, -0.06985947194357875, 0.0, -0.12446173084176845, -1.5808415926365578, 0.014333078135875619, -0.0806417043955706, -0.37124401660668394, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.25955282922987977, -0.1618313202464181, 0.002447883426630002, -0.5149037074691503, -0.00010703274362664899, 0.0008742582163227642, 0.10168585913285667, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.785398163397, 0.32250041955468695, -0.2569883469655496, 0.18577095561452217, 1.164388709412583, 0.0694401264431558, 0.5475575114527793, -0.11842286843715424, 0.8254301089264399]
half_sitting = [
-0.74,0,1.0192720229567027,0,0,0,1, # root_joint
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right
0, 0.006761, # torso
0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left
0, 0, 0, 0, 0, 0, 0, # gripper_left
left_gripper_lock + right_gripper_lock))
graph.addConstraints(
node="free",
constraints=Constraints(numConstraints=com_constraint + foot_placement +
left_gripper_lock + right_gripper_lock +
gaze_constraint + waist_constraint))
graph.addConstraints(edge="Loop | f",
constraints=Constraints(numConstraints=com_constraint +
foot_placement + arm_locked))
graph.initialize()
ps.setParameter("SimpleTimeParameterization/safety", 0.5)
ps.setParameter("SimpleTimeParameterization/order", 2)
ps.setParameter("SimpleTimeParameterization/maxAcceleration", .25)
ps.addPathOptimizer("RandomShortcut")
ps.addPathOptimizer("SimpleTimeParameterization")
res, q, err = graph.generateTargetConfig("starting_motion", initConf, initConf)
if not res:
raise RuntimeError('Failed to project initial configuration')
ps.setRandomSeed(54)
ps.setInitialConfig(initConf)
ps.addGoalConfig(q)
ps.solve()
# Delete intermediate non optimized paths
for i in range(ps.numberPaths() - 1):
ps.erasePath(0)
# Generate N random configurations
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()
q0[:4] = [-3., 2., 0, -1]
#q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15
q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.34
q0[robot.rankInConfiguration['tiago/arm_1_joint']] = 0.10
# create intermediate touchpoints configuration
success3, q_touch_1, residual_error3 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_top", q_init) # Collision between legs
success4, q_touch_2, residual_error4 = graph.applyNodeConstraints("talos/right_gripper grasps desk/touchpoint_right_front", q_init) # Collision between legs
success1, q_touch_3, residual_error1 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_front", q_init) # Collision between legs
success2, q_touch_4, residual_error2 = graph.applyNodeConstraints("talos/left_gripper grasps desk/touchpoint_left_drawer", q_init) # fail
# q_touch_1 redefinition
q_touch_1 = coda.q_touch_1
# q_touch_2 redefinition
q_touch_2 = coda.q_touch_2
# q_touch_4 redefinition
q_touch_4 = coda.q_touch_4
ps.addPathOptimizer("Graph-RandomShortcut")
from hpp.gepetto import PathPlayer
pp = PathPlayer(v, ps.client.basic)
def solveAll():
# trajectory 1
ps.setInitialConfig(q_init)
ps.addGoalConfig(q_touch_1)
graph.setWeight("talos/right_gripper > desk/touchpoint_right_front | f", 0)
graph.setWeight("talos/left_gripper > desk/touchpoint_left_front | f", 0)
graph.setWeight("talos/left_gripper > desk/touchpoint_left_drawer | f", 0)
graph.setWeight("talos/right_gripper > desk/upper_drawer_spherical_handle | f", 0)
print ps.solve()
# trajectory 2
