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 initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
'tiago/hand_ring_flex_2_joint',
'tiago/hand_ring_virtual_3_joint',
'tiago/hand_ring_flex_3_joint',
'tiago/hand_thumb_abd_joint',
'tiago/hand_thumb_virtual_1_joint',
'tiago/hand_thumb_flex_1_joint',
'tiago/hand_thumb_virtual_2_joint',
'tiago/hand_thumb_flex_2_joint',
]
crobot.removeJoints(removedJoints, qneutral)
tiago_fov.reduceModel(removedJoints, qneutral, len_prefix=len("tiago/"))
del crobot
robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/tiago.srdf")
robot.insertRobotSRDFModel("tiago", "package://tiago_data/srdf/pal_hey5_gripper.srdf")
robot.setJointBounds('tiago/root_joint', [-10, 10, -10, 10])
ps = ProblemSolver(robot)
ps.loadPlugin("manipulation-spline-gradient-based.so")
vf = ViewerFactory(ps)
vf.loadRobotModel (Driller, "driller")
robot.insertRobotSRDFModel("driller", "package://gerard_bauzil/srdf/qr_drill.srdf")
robot.setJointBounds('driller/root_joint', [-10, 10, -10, 10, 0, 2])
vf.loadRobotModel (PartP72, "part")
robot.setJointBounds('part/root_joint', [-5, 5, -5, 5, -5, 5])
srdf_disable_collisions_fmt = """ <disable_collisions link1="{}" link2="{}" reason=""/>\n"""
# Disable collision between tiago/hand_safety_box_0 and driller
srdf_disable_collisions = """<robot>"""
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/base_link")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_top")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_back")
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createRealClient ()
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph (robot, 'graph')
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
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createRealClient ()
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2])
# 2}}}
# 2}}}
from hpp.gepetto.manipulation import Viewer
class ScrewGun (object):
rootJointType = 'freeflyer'
packageName = 'airbus_environment'
meshPackageName = 'airbus_environment'
urdfName = 'screw_gun'
urdfSuffix = "_massless"
srdfSuffix = ""
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2-screw', 'hrp2')
ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
r = Viewer (ps)
r.loadObjectModel (ScrewGun, 'screw_gun')
r.buildCompositeRobot (['hrp2', 'screw_gun'])
for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathOptimizer ('None')
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'cup'
urdfSuffix = ""
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.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735,
0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5,
0.02916333341652683, 0.025597731231524482, 0.16145134862579935,
-2.785939904956431, -4.563075760833335, 0.8958690128585236,
-0.19634763254425533, -0.7205092487114027, 0.6650461296003519,
0.005260724207565836
]
# 3}}}
robot.client.basic.problem.resetRoadmap()
robot.client.basic.problem.selectPathOptimizer('None')
robot.client.basic.problem.setErrorThreshold(1e-3)
robot.client.basic.problem.setMaxIterations(40)
from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver(robot)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph(robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint(
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface',
'pancake_table_table_top')
jointNames = dict()
jointNames['all'] = robot.getJointNames()
jointNames['pr2'] = list()
jointNames['allButPR2LeftArm'] = list()
for n in jointNames['all']:
if n.startswith("pr2"):
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 {{{3
robot = Robot ('hrp2', 'hrp2')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointBounds ("hrp2/base_joint_xyz", [-0.2,0.8,-0.5,0.5, 0,2])
ps.selectPathProjector ('Progressive', 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Open hands
ilh = robot.rankInConfiguration ['hrp2/LARM_JOINT6']
q_init [ilh:ilh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration ['hrp2/RARM_JOINT6']
q_init [irh:irh+6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration ['hrp2/base_joint_xyz']
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('pr2-box', 'pr2')
ps = ProblemSolver(robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createViewer ()
vf = ViewerFactory(ps)
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# 2}}}
# 1}}}
4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75,
-0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975,
-0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136,
0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679,
6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079,
-0.0011302430393443214, 0.00792613666021656, -0.05431204743596144,
-0.020779247681766293, 0.026905347698115048, -0.00494217897984072,
-0.0011303560351055939, 0.007923626259361372, -0.05448958126663155,
-0.019807309704424767, 0.02612488205235436, -0.004939666182290102,
1.9999999999999933, -0.009968182216395699, 0.3399077826251962,
0.8920410834920809, 0.03081996727219969, -0.37686669705702674,
-0.2475567159843218
]
#qgoal=
p = ProblemSolver(robot)
p.createPreGrasp('left-hand-pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createPreGrasp('left-hand-pregrasp-passive', 'hrp2/leftHand',
'screw_gun/handle2')
p.createGrasp('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createGrasp('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2')
p.createLockedDofConstraint('screwgun_lock_x', 'screw_gun/base_joint_x', 0, 0,
0)
p.createLockedDofConstraint('screwgun_lock_y', 'screw_gun/base_joint_y', 0, 0,
0)
p.createLockedDofConstraint('screwgun_lock_z', 'screw_gun/base_joint_z', 0, 0,
0)
p.createLockedDofConstraint('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0,
1, 0)
p.createLockedDofConstraint('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0,
2, 1)
width = bounds[1] - bounds[0]
mean = .5 * (bounds[1] + bounds[0])
m = mean - .5 * ratio * width
M = mean + .5 * ratio * width
robot.setJointBounds(j, [m, M])
print("context=" + args.context)
loadServerPlugin(args.context, "manipulation-corba.so")
client = CorbaClient(context=args.context)
client.manipulation.problem.selectProblem(args.context)
robot = Robot("robot", "tiago", rootJointType="planar", client=client)
robot.setJointBounds('tiago/root_joint', [-2, 2, -2, 2])
#robot.insertRobotSRDFModel("tiago", "tiago_data", "schunk", "_gripper")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadRobotModel(Driller, "driller")
robot.insertRobotSRDFModel("driller", "gerard_bauzil", "qr_drill", "")
robot.setJointBounds('driller/root_joint', [-2, 2, -2, 2, 0, 2])
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)
class Cup(object):
rootJointType = "freeflyer"
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']
from hpp.gepetto import PathPlayer, PathPlayerGui
class ScrewGun (object):
rootJointType = 'freeflyer'
packageName = 'airbus_environment'
meshPackageName = 'airbus_environment'
urdfName = 'screw_gun'
urdfSuffix = ""
srdfSuffix = ""
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 and a screwgun {{{3
robot = Robot ('hrp2-screw', 'hrp2')
ps = ProblemSolver (robot)
ps.selectPathPlanner ("M-RRT")
vf = ViewerFactory (ps)
vf.loadObjectModel (ScrewGun, 'screw_gun')
for d in ["hrp2", "screw_gun"]:
robot.setJointBounds (d+"/base_joint_xyz", [-4,4,-4,4,-4,4])
ps.selectPathProjector ("Progressive", 0.2)
ps.setErrorThreshold (1e-3)
ps.setMaxIterations (40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig ()
q_init = half_sitting [::]
# Set initial position of screw-driver
packageName = 'hpp_practicals'
meshPackageName = 'hpp_practicals'
urdfName = 'ur_benchmark/pokeball'
urdfSuffix = ""
srdfSuffix = ""
class Ground (object):
rootJointType = 'anchor'
packageName = 'hpp_practicals'
urdfName = 'ur_benchmark/ground'
meshPackageName = 'hpp_practicals'
urdfSuffix = ""
srdfSuffix = ""
class Box (object):
rootJointType = 'anchor'
packageName = 'hpp_practicals'
urdfName = 'ur_benchmark/box'
meshPackageName = 'hpp_practicals'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('ur5-pokeball', 'ur5')
ps = ProblemSolver (robot)
ps.setErrorThreshold (1e-4)
ps.setMaxIterProjection (40)
vf = ViewerFactory (ps)
gripperName = 'ur5/wrist_3_joint'
ballName = 'pokeball/root_joint'
0, 0, 1.095, 0, 0, 0, 1, 0.0, 0.0, -0.411354, 0.859395, -0.448041,
-0.001708, 0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, 0,
0.006761, 0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, 0, 0, 0, 0, 0,
0, 0, -0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, 0, 0, 0, 0, 0, 0,
0, 0, 0
]
robot = HumanoidRobot("talos", "talos", rootJointType="freeflyer")
shrinkJointRange(robot, 0.95)
# set freeflyerjoint bounds
robot.setJointBounds("talos/root_joint", [
-0.5, 0.5, -0.5, 0.5, 0.5, 1.5, -1.01, 1.01, -1.01, 1.01, -1.01, 1.01,
-1.01, 1.01
])
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-4)
ps.setMaxIterProjection(40)
vf = ViewerFactory(ps)
left_arm_lock = createLeftArmLockedJoints(ps)
right_arm_lock = createRightArmLockedJoints(ps)
if args.arm == 'left':
arm_locked = right_arm_lock
elif args.arm == 'right':
arm_locked = left_arm_lock
else:
arm_locked = list()
left_gripper_lock, right_gripper_lock = createGripperLockedJoints(ps, initConf)
q_goal [rank:rank+3] = [-2.5, -4.4, 0.76]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
q_goal [rank:rank+4] = [c, 0, -c, 0]
r (q_goal)
del c
q_inter = [-0.8017105239677402, -0.5977125025958312, -0.796440524800078, 0.6047168680102916, -0.4879605145323316, 0.8728657034488995, -0.988715265911429, 0.1498069522875767, -0.012487804646172175, -0.9999220243274567, -0.9479271854727237, -0.31848712853388694, 0.06700549180802896, -0.9977526066453368, 0.15793164217459785, 0.9874500475467277, 0.9804271799015071, -0.19688205837601827, 0.06981400674906149, 0.9975600254930236, 0.8026666074307995, -0.5964279649006498, -0.8558688410761539, -0.5171929300318802, 0.07633365848037467, 2.5514381844999448, 1.1951774265118278, -0.5864281075389233, 0.24807300661555917, 1.0730239901832896, -0.9931474461781542, 0.5380253563010143, -0.8429286541440898, 0.0, -0.9291311234626017, 0.36975039609596555, 0.5, 0.07192830903452277, 0.0516497980242827, 0.5, 0.2978673015357309, 0.011873305063635719, 0.2207828272342602, 0.9968680838221816, -1.1330407965502385, 0.1474961939381539, -0.9059397450606351, -0.9591666722669869, 0.8241613711518598, -0.5663550426199861, -2.094, 0.7924979452316735, 0.6098745828476339, 0.5, 0.35889873246983567, 0.10845342945887403, 0.5, 0.02916333341652683, 0.025597731231524482, 0.16145134862579935, -2.785939904956431, -4.563075760833335, 0.8958690128585236, -0.19634763254425533, -0.7205092487114027, 0.6650461296003519, 0.005260724207565836]
# 3}}}
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
from hpp.corbaserver.manipulation import ProblemSolver
p = ProblemSolver (robot)
# Create constraints. {{{3
from hpp.corbaserver.manipulation import ConstraintGraph
cg = ConstraintGraph (robot, 'graph')
robot.client.manipulation.problem.createPlacementConstraint (
'box_placement', 'box', 'box/base_joint_SO3', 'box_surface', 'pancake_table_table_top')
jointNames = dict ()
jointNames['all'] = robot.getJointNames ()
jointNames['pr2'] = list ()
jointNames['allButPR2LeftArm'] = list ()
for n in jointNames['all']:
if n.startswith ("pr2"):
jointNames['pr2'].append (n)
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto import PathPlayer
from hpp.gepetto.manipulation import ViewerFactory
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('tom-box', 'tom')
robot.setJointBounds ('tom/base_joint_xy', [-1,1,-1,1])
ps = ProblemSolver (robot)
fk = ViewerFactory (ps)
fk.loadObjectModel (Box, 'box')
robot.setJointBounds ('box/base_joint_xyz', [-1,1,-1,1,-1,1])
openLeftHand = []
for j, v in robot.openHand (None, .75, "left").iteritems () :
lockedJointName = "open/"+j
openLeftHand.append (lockedJointName)
ps.createLockedJoint (lockedJointName, j, [v,])
openRightHand = []
for j, v in robot.openHand (None, .75, "right").iteritems () :
lockedJointName = "open/"+j
openRightHand.append (lockedJointName)
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createViewer ()
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint", [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5])
# 2}}}
# 1}}}
# vim: foldmethod=marker foldlevel=2
from hpp.corbaserver.manipulation.hrp2 import Robot
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph
from hpp.gepetto.manipulation import Viewer, ViewerFactory
from hpp.gepetto import PathPlayer, PathPlayerGui
Robot.urdfSuffix = '_capsule_mesh'
Robot.srdfSuffix = '_manipulation'
# Load HRP2 {{{3
robot = Robot('hrp2', 'hrp2')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
robot.setJointBounds("hrp2/base_joint_xyz", [-0.2, 0.8, -0.5, 0.5, 0, 2])
ps.selectPathProjector('Progressive', 0.2)
ps.setErrorThreshold(1e-3)
ps.setMaxIterations(40)
# 3}}}
# Define configurations {{{3
half_sitting = q = robot.getInitialConfig()
q_init = half_sitting[::]
# Open hands
ilh = robot.rankInConfiguration['hrp2/LARM_JOINT6']
q_init[ilh:ilh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
irh = robot.rankInConfiguration['hrp2/RARM_JOINT6']
q_init[irh:irh + 6] = [0.75, -0.75, 0.75, -0.75, 0.75, -0.75]
ibjxyz = robot.rankInConfiguration['hrp2/base_joint_xyz']
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
boxes.append ("box" + str(i))
vf.loadObjectModel (Box, boxes[i])
robot.setJointBounds (boxes[i]+ '/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
def setBoxColors (gui):
c = Color()
for i in xrange(K):
gui.setColor (boxes[i], c[i])
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
vf.loadObjectModel (Box, "box1")
vf.loadObjectModel (Box, "box2")
robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
class Cup (object):
rootJointType = "freeflyer"
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']
urdfName = 'placard'
urdfSuffix = ''
srdfSuffix = ''
def __init__(self, name, vf):
self.name = name
self.vf = vf
self.joints = [name + '/root_joint']
self.handles = dict()
self.handles['low'] = name + '/low'
self.handles['high'] = name + '/high'
vf.loadObjectModel(self.__class__, name)
self.rank = vf.robot.rankInConfiguration[name + '/root_joint']
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.,
rootJointType = "freeflyer"
packageName = "hpp_tutorial"
urdfName = "box"
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = "hpp_tutorial"
urdfName = "kitchen_area"
urdfSuffix = ""
srdfSuffix = ""
robot = Robot("pr2-box", "pr2")
ps = ProblemSolver(robot)
# ViewerFactory is a class that generates Viewer on the go. It means you can
# restart the server and regenerate a new windows.
# To generate a window:
# vf.createViewer ()
vf = ViewerFactory(ps)
vf.loadObjectModel(Box, "box")
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# Initialization.
# Set parameters.
Robot.packageName = 'ur_description'
Robot.meshPackageName = 'ur_description'
Robot.urdfName = 'ur5'
Robot.urdfSuffix = '_robot'
Robot.srdfSuffix = ''
class Box (object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('ur5-box', 'ur5', rootJointType = 'anchor')
ps = ProblemSolver (robot)
fk = ViewerFactory (ps)
fk.loadObjectModel (Box, 'box')
fk.buildCompositeRobot (['ur5', 'box'])
robot.setJointBounds ("box/base_joint_xyz", [-2,+2,-2,+2,-2,2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['ur5/shoulder_lift_joint']
q_init [rank] = 0
rank = robot.rankInConfiguration ['ur5/elbow_joint']
q_init [rank] = - PI / 2
rank = robot.rankInConfiguration ['box/base_joint_xyz']
q_init [rank:rank+3] = [1,1,1]
robot.client.basic.problem.selectPathPlanner ("M-RRT")
robot.loadObjectModel ('box', 'freeflyer', 'fiad_description', 'box', '', '')
robot.buildCompositeRobot ('robot', ['ur5', 'box'])
for a in ["x","y","z"]:
robot.setJointBounds ("box/base_joint_"+a, [-1,1])
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setMaxIterations (40)
r = MultiRobotPub (robot)
half_sitting = robot.getCurrentConfig ();
qinit=[0,0,0,0,0,0,0, 0.5,0,1,0,0,0]
qgoal=[0,0,0,0,0,0,0,-0.5,0,1,0,0,0]
p = ProblemSolver (robot)
p.createGrasp ('grasp', 'ur5/ee', 'box/handle')
p.createGrasp ('grasp-passive', 'ur5/ee', 'box/handle')
if withWaypoint:
p.createPreGrasp ('pregrasp', 'ur5/ee', 'box/handle')
p.createLockedDofConstraint ('box_lock_x' , 'box/base_joint_x' , 0, 0, 0)
p.createLockedDofConstraint ('box_lock_y' , 'box/base_joint_y' , 0, 0, 0)
p.createLockedDofConstraint ('box_lock_z' , 'box/base_joint_z' , 0, 0, 0)
p.createLockedDofConstraint ('box_lock_rx', 'box/base_joint_SO3', 0, 1, 0)
p.createLockedDofConstraint ('box_lock_ry', 'box/base_joint_SO3', 0, 2, 1)
p.createLockedDofConstraint ('box_lock_rz', 'box/base_joint_SO3', 0, 3, 2)
p.isLockedDofParametric ('box_lock_x' ,True)
p.isLockedDofParametric ('box_lock_y' ,True)
p.isLockedDofParametric ('box_lock_z' ,True)
p.isLockedDofParametric ('box_lock_rx',True)
p.isLockedDofParametric ('box_lock_ry',True)
srdfFilename = \
"package://hpp_environments/srdf/construction_set/sphere.srdf"
class Ground(object):
rootJointType = 'freeflyer'
urdfFilename = \
"package://hpp_environments/urdf/construction_set/ground.urdf"
srdfFilename = \
"package://hpp_environments/srdf/construction_set/ground.srdf"
robot = Robot('2ur5-sphere', 'r0')
robot.setJointPosition('r0/root_joint', [-.25, 0, 0, 0, 0, 0, 1])
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-4)
ps.setMaxIterProjection(40)
vf = ViewerFactory(ps)
#
# Load robots and objets
# - 2 UR3,
# - 4 spheres,
# - 4 short cylinders,
vf.loadRobotModel(Robot, "r1")
robot.setJointPosition('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0])
# Change bounds of robots to increase workspace and avoid some collisions
def benchConstraints (constraints, lockDofs):
ps.client.basic.problem.resetConstraints()
ps.setNumericalConstraints ("test", constraints)
ps.setLockedJointConstraints ("test", lockDofs)
res = [None] * N
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)
ps.client.basic.problem.resetConstraints()
ps.resetConstraints()
ps.addNumericalConstraints("test", constraints)
ps.addLockedJointConstraints("test", lockDofs)
res = [None] * N
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.,
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot ('pr2-box', 'pr2') # was anchor joint
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
q_init[0:4] = [-3.2,-4,1,0] # FIX ME ! (see up )
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint']
q_init [rank] = 0.5
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 0, 0, 0, 1])
vf.loadEnvironmentModel (Table, "table")
boxes = list()
for i in xrange(K):
boxes.append ("box" + str(i))
vf.loadObjectModel (Box, boxes[i])
robot.setJointBounds (boxes[i]+ '/root_joint', [-1,0.5,-1,2,0.6,1.9,-1,1,-1,1,-1,1,-1,1])
def setBoxColors (gui):
c = Color()
for i in xrange(K):
gui.setColor (boxes[i], c[i])
class Box(object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot('baxter-manip', 'baxter')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition("baxter", [-0.8, 0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel(Table, "table")
vf.loadObjectModel(Box, "box1")
vf.loadObjectModel(Box, "box2")
robot.setJointBounds('box1/base_joint_xyz', [-1, 0.5, -1, 2, 0.6, 1.9])
robot.setJointBounds('box2/base_joint_xyz', [-1, 0.5, -1, 2, 0.6, 1.9])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
class Calibration(object):
def __init__(self, client):
self.client = client
self.robot = None
self.robots = list()
self.robot_locks = list()
self.robots_locks = list()
self.robots_gaze = list()
self.robots_identity_constraints = list()
self.q = []
def initRobot(self):
# The first robot is loaded differently by hpp
robot_id = len(self.robots) + int(self.robot is not None)
robot_name = "talos_" + str(robot_id)
if self.robot is None:
Robot.packageName = "agimus_demos"
Robot.urdfName = "talos"
Robot.urdfSuffix = "_calibration_camera"
Robot.srdfSuffix = ""
self.robot = Robot(
"talos", robot_name, rootJointType="freeflyer", client=self.client
)
self.ps = ProblemSolver(self.robot)
self.ps.setErrorThreshold(0.00000001)
self.ps.setMaxIterProjection(100)
self.vf = ViewerFactory(self.ps)
else:
self.robots.append(OtherRobot(name=robot_name, vf=self.vf))
self.robot.setJointBounds(robot_name + "/root_joint", [-1, 1, -1, 1, 0.5, 1.5])
rank, size = self.getRankAndConfigSize(robot_id)
self.q[rank : rank + size] = self.robot.getCurrentConfig()[rank : rank + size]
return robot_id
def setLockJoints(self, robot_id, joints_name_value_tuple):
self.ps.createLockedJoint(
"talos_" + str(robot_id) + "/root_joint",
"talos_" + str(robot_id) + "/root_joint",
[0, 0, 1, 0, 0, 0, 1],
)
root_joint_rank = self.robot.rankInConfiguration[
"talos_" + str(robot_id) + "/root_joint"
]
self.q[root_joint_rank : root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1]
self.robots_locks.append("talos_" + str(robot_id) + "/root_joint")
# self.ps.createLockedJoint("talos_" + str(robot_id) + "/calib_mire_joint_2", "talos_" + str(robot_id) + "/calib_mire_joint_2", [0, 0, 0, 0, 0, 0, 1])
# mire_joint_rank = self.robot.rankInConfiguration["talos_" + str(robot_id) + "/calib_mire_joint_2"]
# self.q[mire_joint_rank:mire_joint_rank + 7] = [0, 0, 0, 0, 0, 0, 1]
# self.robots_locks.append("talos_" + str(robot_id) + "/calib_mire_joint_2")
for name, value in joints_name_value_tuple:
joint_name = "talos_" + str(robot_id) + "/" + name
self.q[self.robot.rankInConfiguration[joint_name]] = value
self.ps.createLockedJoint(joint_name, joint_name, [value])
self.robots_locks.append(joint_name)
def getRankAndConfigSize(self, robot_id):
robot_name = "talos_" + str(robot_id)
rank = self.robot.rankInConfiguration[robot_name + "/root_joint"]
size = sum(
[
self.robot.getJointConfigSize(joint_name)
for joint_name in calib.robot.getJointNames()
if robot_name in joint_name
]
)
return rank, size
def setGaze(self, robot_id, checkerboard_pose):
robot_name = "talos_" + str(robot_id)
position = (
checkerboard_pose.position.x,
checkerboard_pose.position.y,
checkerboard_pose.position.z,
)
orientation = (
checkerboard_pose.orientation.x,
checkerboard_pose.orientation.y,
checkerboard_pose.orientation.z,
checkerboard_pose.orientation.w,
)
self.ps.createPositionConstraint(
robot_name + "gaze",
robot_name + "/calib_rgb_joint",
robot_name + "/calib_mire_joint_2",
position,
(0, 0, 0),
[True, True, True],
)
self.ps.createOrientationConstraint(
robot_name + "gaze_O",
robot_name + "/calib_mire_joint_2",
robot_name + "/calib_rgb_joint",
orientation,
[True, True, True],
)
self.robots_gaze.append(robot_name + "gaze")
self.robots_gaze.append(robot_name + "gaze_O")
def constrainFreeflyers(self):
for robot_id in range(1, len(self.robots)):
robot_name = "talos_" + str(robot_id)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_rgb",
["talos_0/calib_rgb_joint"],
[robot_name + "/calib_rgb_joint"],
)
self.client.basic.problem.createIdentityConstraint(
robot_name + "_id_mire",
["talos_0/calib_mire_joint_2"],
[robot_name + "/calib_mire_joint_2"],
)
self.robots_identity_constraints.append(robot_name + "_id_rgb")
self.robots_identity_constraints.append(robot_name + "_id_mire")
def optimize(self, q_init, robots_id):
self.q_proj = self.q
client.basic.problem.resetConstraints()
calib.robot.setCurrentConfig(q_init)
client.basic.problem.addLockedJointConstraints("unused1", calib.robots_locks)
gaze = [
c
for c in calib.robots_gaze
if any(["talos_" + str(robot_id) in c for robot_id in robots_id])
]
num_constraints = gaze + calib.robots_identity_constraints
client.basic.problem.addNumericalConstraints(
"unused2", num_constraints, [0 for _ in num_constraints]
)
# client.basic.problem.addNumericalConstraints("mighty_cam_cost", ["cam_cost"], [ 1 ])
# calib.client.basic.problem.setNumericalConstraintsLastPriorityOptional(True)
projOk, self.q_proj, error = client.basic.problem.applyConstraints(q_init)
if error < 1.0:
optOk, self.q_proj, error = client.basic.problem.optimize(self.q_proj)
if optOk:
print("All was good! " + str(max(error)))
else:
print("Optimisation error: " + str(max(error)))
else:
print("Projection error: " + str(error))
rank_rgb = calib.robot.rankInConfiguration["talos_0/calib_rgb_joint"]
rank_mire = calib.robot.rankInConfiguration["talos_0/calib_mire_joint_2"]
return (
self.q_proj,
self.q_proj[rank_rgb : rank_rgb + 7],
max(error),
self.q_proj[rank_mire : rank_mire + 7],
)
packageName = 'hpp_environments'
urdfName = 'construction_set/sphere'
urdfSuffix = ""
srdfSuffix = ""
class Ground (object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'construction_set/ground'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('2ur5-sphere', 'r0')
robot.setJointPosition ('r0/root_joint', [-.25, 0, 0, 0, 0, 0, 1])
ps = ProblemSolver (robot)
ps.setErrorThreshold (1e-4)
ps.setMaxIterProjection (40)
vf = ViewerFactory (ps)
#
# Load robots and objets
# - 2 UR3,
# - 4 spheres,
# - 4 short cylinders,
vf.loadRobotModel (Robot, "r1")
robot.setJointPosition ('r1/root_joint', [.25, 0, 0, 0, 0, 1, 0])
# Change bounds of robots to increase workspace and avoid some collisions
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('pr2-box', 'pr2')
ps = ProblemSolver(robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## vf.createRealClient ()
vf = ViewerFactory(ps)
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7, -2, 2, -2, 2])
robot.setJointBounds(
"box/root_joint",
[-5.1, -2, -5.2, -2.7, 0, 1.5, -2, 2, -2, 2, -2, 2, -2, 2])
# 2}}}
from hpp.corbaserver.manipulation import ProblemSolver, ConstraintGraph, ConstraintGraphFactory, Rule, Constraints
from hpp.gepetto.manipulation import ViewerFactory
from hpp import Transform
from desk_robot import Robot as Desk
from talos_robot import Robot as Talos
import math
import numpy as np
import CORBA
import config_data as coda
# Load robot (talos + desk)
robot = Talos("talos+desk", "talos")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObjectModel (Desk, "desk")
v = vf.createViewer()
robot.setJointBounds("talos/root_joint", [-1.5, 1.5, -2, 1, 0.5, 1.5])
q = robot.getCurrentConfig()
pi = 3.141592654
q[0] = 0.3
q[1] = -1.4
q[2] = 0.921
q[5] = math.sin(0.5 * (pi/2))
q[6] = math.cos(0.5 * (pi/2))
# hips offset
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot ('pr2-box', 'pr2', rootJointType = "anchor")
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
robot.setJointPosition ('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0))
vf.loadObjectModel (Box, 'box')
vf.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("box/base_joint_xyz", [-3,-2,-5,-3,0.7,1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rank = robot.rankInConfiguration ['pr2/r_gripper_l_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/r_gripper_r_finger_joint']
q_init [rank] = 0.5
rank = robot.rankInConfiguration ['pr2/l_gripper_l_finger_joint']
Robot.urdfName = 'ur5'
Robot.urdfSuffix = '_robot'
Robot.srdfSuffix = ''
class Box(object):
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('ur5-box', 'ur5', rootJointType='anchor')
ps = ProblemSolver(robot)
fk = ViewerFactory(ps)
fk.loadObjectModel(Box, 'box')
fk.buildCompositeRobot(['ur5', 'box'])
robot.setJointBounds("box/base_joint_xyz", [-2, +2, -2, +2, -2, 2])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
rank = robot.rankInConfiguration['ur5/shoulder_lift_joint']
q_init[rank] = 0
rank = robot.rankInConfiguration['ur5/elbow_joint']
q_init[rank] = -PI / 2
rank = robot.rankInConfiguration['box/base_joint_xyz']
q_init[rank:rank + 3] = [1, 1, 1]
class Box(object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot('baxter-manip', 'baxter')
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition("baxter", [-0.8, 0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel(Table, "table")
boxes = list()
for i in xrange(K):
boxes.append("box" + str(i))
vf.loadObjectModel(Box, boxes[i])
robot.setJointBounds(boxes[i] + '/base_joint_xyz',
[-1, 0.5, -1, 2, 0.6, 1.9])
def setBoxColors(gui):
c = Color()
urdfSuffix = ""
srdfSuffix = ""
class Ground(object):
rootJointType = 'anchor'
packageName = 'hpp_practicals'
urdfName = 'ur_benchmark/ground'
meshPackageName = 'hpp_practicals'
urdfSuffix = ""
srdfSuffix = ""
class Box(object):
rootJointType = 'anchor'
packageName = 'hpp_practicals'
urdfName = 'ur_benchmark/box'
meshPackageName = 'hpp_practicals'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('ur5-pokeball', 'ur5')
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-4)
ps.setMaxIterProjection(40)
vf = ViewerFactory(ps)
gripperName = 'ur5/wrist_3_joint'
ballName = 'pokeball/root_joint'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot('pr2-box', 'pr2', rootJointType="anchor")
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
robot.setJointPosition('pr2/base_joint', (-3.2, -4, 0, 1, 0, 0, 0))
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("box/base_joint_xyz", [-3, -2, -5, -3, 0.7, 1])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint']
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot('pr2-box', 'pr2')
ps = ProblemSolver(robot)
## ViewerFactory is a class that generates Viewer on the go. It means you can
## restart the server and regenerate a new windows.
## To generate a window:
## fk.createRealClient ()
fk = ViewerFactory(ps)
fk.loadObjectModel(Box, 'box')
fk.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/base_joint_xy", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/base_joint_xyz", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph(robot, 'graph')
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
vf.loadObjectModel (Box, "box1")
vf.loadObjectModel (Box, "box2")
robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB1 = robot.rankInConfiguration ['box1/base_joint_xyz']
rootJointType = "freeflyer"
packageName = "agimus_demos"
urdfName = "calibration_mire"
urdfSuffix = ""
srdfSuffix = ""
name = "mire"
handles = ["mire/left", "mire/right"]
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)
vf.loadObjectModel(Mire, "mire")
robot.setJointBounds("mire/root_joint", [-1, 1, -1, 1, 0, 2])
half_sitting = [
0,
0,
1.0192720229567027,
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment(object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
# Load robot and object. {{{3
robot = Robot('pr2-box', 'pr2') # was anchor joint
ps = ProblemSolver(robot)
vf = ViewerFactory(ps)
vf.loadObjectModel(Box, 'box')
vf.loadEnvironmentModel(Environment, "kitchen_area")
robot.setJointBounds("pr2/root_joint", [-5, -2, -5.2, -2.7])
robot.setJointBounds("box/root_joint", [-5.1, -2, -5.2, -2.7, 0, 1.5])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig()
q_init[0:4] = [-3.2, -4, 1, 0] # FIX ME ! (see up )
rank = robot.rankInConfiguration['pr2/r_gripper_l_finger_joint']
q_init[rank] = 0.5
rank = robot.rankInConfiguration['pr2/r_gripper_r_finger_joint']
q_init[rank] = 0.5
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
rootJointType = 'freeflyer'
packageName = 'hpp_tutorial'
meshPackageName = 'hpp_tutorial'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
class Environment (object):
packageName = 'iai_maps'
meshPackageName = 'iai_maps'
urdfName = 'kitchen_area'
urdfSuffix = ""
srdfSuffix = ""
robot = Robot ('pr2-box', 'pr2')
ps = ProblemSolver (robot)
fk = FakeViewer (ps)
fk.loadObjectModel (Box, 'box')
fk.buildCompositeRobot (['pr2', 'box'])
fk.loadEnvironmentModel (Environment, "kitchen_area")
robot.setJointBounds ("pr2/base_joint_xy" , [-5,-2,-5.2,-2.7] )
robot.setJointBounds ("box/base_joint_xyz", [-5.1,-2,-5.2,-2.7,0,1.5])
# 2}}}
# Load the Python class ConstraintGraph. {{{2
graph = ConstraintGraph (robot, 'graph')
# 2}}}
# 1}}}
srdfSuffix = ""
class Box (object):
rootJointType = "freeflyer"
packageName = 'hpp-baxter'
meshPackageName = 'hpp-baxter'
urdfName = 'box'
urdfSuffix = ""
srdfSuffix = ""
joint = "base_joint"
handle = "handle"
# 4}}}
Robot.urdfSuffix = ""
robot = Robot ('baxter-manip', 'baxter')
ps = ProblemSolver (robot)
vf = ViewerFactory (ps)
#robot.setRootJointPosition ("baxter" , [-3.2,-3.9, 0.926, 1, 0, 0, 0])
robot.setRootJointPosition ("baxter" , [-0.8,0.8, 0.926, 1, 0, 0, 0])
vf.loadEnvironmentModel (Table, "table")
vf.loadObjectModel (Box, "box1")
vf.loadObjectModel (Box, "box2")
robot.setJointBounds ('box1/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
robot.setJointBounds ('box2/base_joint_xyz', [-1,0.5,-1,2,0.6,1.9])
# 3}}}
# Define configurations. {{{3
q_init = robot.getCurrentConfig ()
rankB1 = robot.rankInConfiguration ['box1/base_joint_xyz']
urdfSuffix = ""
srdfSuffix = ""
class Ground(object):
rootJointType = 'anchor'
packageName = 'hpp_environments'
urdfName = 'construction_set/ground'
urdfSuffix = ""
srdfSuffix = ""
nSphere = 2
robot = Robot('ur3-spheres', 'ur3')
ps = ProblemSolver(robot)
ps.setErrorThreshold(1e-4)
ps.setMaxIterProjection(40)
vf = ViewerFactory(ps)
# Change bounds of robots to increase workspace and avoid some collisions
robot.setJointBounds('ur3/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds('ur3/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds('ur3/elbow_joint', [-2.6, 2.6])
vf.loadEnvironmentModel(Ground, 'ground')
objects = list()
p = ps.client.basic.problem.getProblem()
r = p.robot()
robot.client.basic.problem.resetRoadmap ()
robot.client.basic.problem.selectPathOptimizer ('None')
robot.client.basic.problem.setErrorThreshold (1e-3)
robot.client.basic.problem.setMaxIterations (40)
r = MultiRobotPub (robot)
half_sitting = robot.getCurrentConfig ();
q1=[0.2561522768052704, 0.009968182216397095, -0.0013343337623661373, 0.9822200021750263, -0.01324065709811156, 0.06488994582367685, 0.1756640181081545, 0.3518980306918394, 0.5587732909530961, -0.00014707196704839167, 0.003348199890377334, 0.2851538622381365, 0.019515985023849783, 0.47568241804625144, 0.034325840512149174, 0.48238375657832383, 0.5252200250587239, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724042397, -0.002329068502171773, 0.0013672024871911833, -0.03184796508559784, 0.00013468971902929027, -0.006463660186047965, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.0014454298679420635, 0.008692815372600783, -0.17944952816665183, -0.0349066, 0.08186718484328222, -0.0052176502711966674, -0.0014458867527617663, 0.008689383322591662, -0.1789250482962942, -0.0349066, 0.0813567181568653, -0.00521418794593098, -1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
q2=[0.2561522768052704, 0.009968182216397094, -0.0013343337623677983, 0.9822200021750275, -0.013240657098102699, 0.06488994582365894, 0.17566401810815518, 0.3518980306918404, 0.5587732909530987, -0.00014707196704828986, 0.003348199890377807, 0.2851538622381267, 0.019515985023862044, 0.4756824180462511, 0.03432584051214365, 0.48238375657832383, 0.5252200250587228, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.08183778724043055, -0.0023290685021591065, 0.0013672024871913863, -0.03184796508560018, 0.00013468971902932646, -0.006463660186048368, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.001445429867963691, 0.008692815372566085, -0.17944952816666074, -0.0349066, 0.08186718484332876, -0.00521765027117088, -0.0014458867527830789, 0.008689383322556907, -0.17892504829630115, -0.0349066, 0.08135671815690743, -0.005214187945905259, 1.0, -0.009968182216397, 0.3399077826256168, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
q3=[0.2561522768052704, 0.009968182216397017, -0.0013072533849402136, 0.9822384333854748, -0.01312068217341367, 0.06461437559545073, 0.17567154051286343, 0.35191845881842604, 0.5584191613132159, -0.00014417077050741273, 0.003264525687460523, 0.2869014360284614, 0.01968512963006628, 0.47563871278773184, 0.0349066, 0.4823374727974044, 0.5253128675582199, 0.00028141477557467405, -9.224156430856334e-05, 9.57339438119022e-05, 0.00043012512285501805, 9.464640215865539e-05, -0.00027454688131241776, -0.08070554282100409, -0.0022150673918241194, 0.0013507126487957277, -0.031445080182081316, 0.00013303660804169588, -0.0063951082465386055, 0.00018071522532470678, -7.008398345853373e-05, 0.0001432918466161822, 5.0663492959904484e-05, 5.932458587315774e-05, -0.0001501990824631989, -0.0016207277515218107, 0.008331445043308924, -0.1782202190053313, -0.0349066, 0.08120820667180957, -0.004973436857255375, -0.0016211611493169267, 0.008328178972275535, -0.17768239866563243, -0.03490564559358417, 0.08066263806839956, -0.00497014215842449, -0.2561522768052704, -0.009968182216396924, 0.33990778262561817, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
qinit=[6.351445422051578e-15, -7.64857075802911e-16, -0.00010650933265040068, 0.9997086753828088, -0.001462864329132029, 0.024091994398753543, 1.4078154478767463e-05, -0.00040620371503693694, 0.06720468781884271, 0.09997150487212819, 0.10619128937732075, -0.030984458364104503, 0.06768151515542697, -9.21491333147205e-05, -0.007395092069129794, 4.108678412429508e-05, 0.09687147183171207, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, -0.030157742792904667, -0.06148082407263112, 0.000178380167127975, -0.006607688410387017, 7.424531689237967e-05, 0.00026391550679217136, 0.0003776033936788175, 0.000250194383634261, -0.0005725514027501679, 6.818331370664385e-05, -0.00020049176175900826, 0.0004808871166639079, -0.0011302430393443214, 0.00792613666021656, -0.05431204743596144, -0.020779247681766293, 0.026905347698115048, -0.00494217897984072, -0.0011303560351055939, 0.007923626259361372, -0.05448958126663155, -0.019807309704424767, 0.02612488205235436, -0.004939666182290102, 1.9999999999999933, -0.009968182216395699, 0.3399077826251962, 0.8920410834920809, 0.03081996727219969, -0.37686669705702674, -0.2475567159843218]
#qgoal=
p = ProblemSolver (robot)
p.createPreGrasp ('left-hand-pregrasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createPreGrasp ('left-hand-pregrasp-passive', 'hrp2/leftHand', 'screw_gun/handle2')
p.createGrasp ('left-hand-grasp', 'hrp2/leftHand', 'screw_gun/handle2')
p.createGrasp ('left-hand-grasp-passive', 'hrp2/leftHand', 'screw_gun/handle2')
p.createLockedDofConstraint ('screwgun_lock_x' , 'screw_gun/base_joint_x' , 0, 0, 0)
p.createLockedDofConstraint ('screwgun_lock_y' , 'screw_gun/base_joint_y' , 0, 0, 0)
p.createLockedDofConstraint ('screwgun_lock_z' , 'screw_gun/base_joint_z' , 0, 0, 0)
p.createLockedDofConstraint ('screwgun_lock_rx', 'screw_gun/base_joint_SO3', 0, 1, 0)
p.createLockedDofConstraint ('screwgun_lock_ry', 'screw_gun/base_joint_SO3', 0, 2, 1)
p.createLockedDofConstraint ('screwgun_lock_rz', 'screw_gun/base_joint_SO3', 0, 3, 2)
p.createLockedDofConstraint ('larm_6' , 'hrp2/LARM_JOINT6' , q1[17], 0, 0)
p.createLockedDofConstraint ('lhand_0', 'hrp2/LHAND_JOINT0', q1[18], 0, 0)
p.createLockedDofConstraint ('lhand_1', 'hrp2/LHAND_JOINT1', q1[19], 0, 0)
p.createLockedDofConstraint ('lhand_2', 'hrp2/LHAND_JOINT2', q1[20], 0, 0)
p.createLockedDofConstraint ('lhand_3', 'hrp2/LHAND_JOINT3', q1[21], 0, 0)
