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
def loadRobot(self):
hppClient = self._hpp()
robotName = rospy.get_param ("/robot_name", "robot")
self.manip.robot.create(robotName)
# Load the robot
robotNames = rospy.get_param ("/robot_names")
self.robot = hpp.corbaserver.manipulation.robot.Robot(robotName, robotNames[0], rootJointType=None, load=False)
self.ps = hpp.corbaserver.manipulation.ProblemSolver (self.robot)
self.vf = ViewerFactory (self.ps)
robots = rospy.get_param ("/robots")
for rn in robotNames:
r = robots[rn]
type = r["type"] if r.has_key("type") else "robot"
if type not in ("robot", "humanoid", "object"):
rospy.logwarn("Param /robots/" + rn + "/type must be one of robot, humanoid or object. Assuming robot")
type = "robot"
if type == "humanoid":
loadFromString = self.manip.robot.insertHumanoidModelFromString
loadFromPack = self.manip.robot.insertHumanoidModel
elif type == "object":
rospy.logwarn("Currently, there is no difference between type robot and type object.)")
loadFromString = self.manip.robot.insertRobotModelFromString
loadFromPack = self.manip.robot.insertRobotModel
else:
loadFromString = self.manip.robot.insertRobotModelFromString
loadFromPack = self.manip.robot.insertRobotModel
try:
loadFromString (rn, r["root_joint_type"], r["description"], r["description_semantic"])
self.vf.loadUrdfInGUI (urdfString, rn)
except KeyError as e1:
try:
loadFromPack (rn, r["root_joint_type"], r["package"], r["urdfName"], r["urdfSuffix"], r["srdfSuffix"])
self.vf.loadUrdfInGUI ("package://" + r["package"] + "/urdf/" + r["urdfName"] + r["urdfSuffix"] + ".urdf", rn)
except KeyError as e2:
rospy.logerr ("Could not load robot " + rn + ":\n" + str(e1) + "\n" + str(e2))
# Set the root joint position in the world.
# base = self.robot.getLinkNames("root_joint")[0]
# p, q = self.tfListener.lookupTransform(self.world_frame, base, rospy.Time(0))
# self.robot.setJointPosition ("root_joint", p + q)
self.robot.rebuildRanks()
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
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
robot.setJointBounds ('r0/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r1/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds ('r0/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds ('r1/shoulder_lift_joint', [-pi, 0])
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.,
2,
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
rank = robot.rankInConfiguration['pr2/l_gripper_l_finger_joint']
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']
rankB2 = robot.rankInConfiguration ['box2/base_joint_xyz']
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
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
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
robot.setJointBounds('r0/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds('r1/shoulder_pan_joint', [-pi, 4])
robot.setJointBounds('r0/shoulder_lift_joint', [-pi, 0])
robot.setJointBounds('r1/shoulder_lift_joint', [-pi, 0])
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,
0,
0,
0,
1, # root_joint
0.0,
0.0,
-0.411354,
0.859395,
class HppServerInitializer(_Parent):
def __init__ (self):
super(HppServerInitializer, self).__init__()
def reset (self, msg):
self.initialize()
self.loadRobot()
if msg.data:
self.loadEnvironment(param="/environment/description", paramName="/environment/name")
self.configure()
self.createGraph("graph")
self.createViewer()
def initialize (self):
self.setHppUrl()
hpp = self._hpp()
# Reset the problem if necessary
if self.robot is not None:
hpp.problem.manipulation.resetProblem()
self.robot = None
self.ps = None
self.vf = None
def loadRobot(self):
hppClient = self._hpp()
robotName = rospy.get_param ("/robot_name", "robot")
self.manip.robot.create(robotName)
# Load the robot
robotNames = rospy.get_param ("/robot_names")
self.robot = hpp.corbaserver.manipulation.robot.Robot(robotName, robotNames[0], rootJointType=None, load=False)
self.ps = hpp.corbaserver.manipulation.ProblemSolver (self.robot)
self.vf = ViewerFactory (self.ps)
robots = rospy.get_param ("/robots")
for rn in robotNames:
r = robots[rn]
type = r["type"] if r.has_key("type") else "robot"
if type not in ("robot", "humanoid", "object"):
rospy.logwarn("Param /robots/" + rn + "/type must be one of robot, humanoid or object. Assuming robot")
type = "robot"
if type == "humanoid":
loadFromString = self.manip.robot.insertHumanoidModelFromString
loadFromPack = self.manip.robot.insertHumanoidModel
elif type == "object":
rospy.logwarn("Currently, there is no difference between type robot and type object.)")
loadFromString = self.manip.robot.insertRobotModelFromString
loadFromPack = self.manip.robot.insertRobotModel
else:
loadFromString = self.manip.robot.insertRobotModelFromString
loadFromPack = self.manip.robot.insertRobotModel
try:
loadFromString (rn, r["root_joint_type"], r["description"], r["description_semantic"])
self.vf.loadUrdfInGUI (urdfString, rn)
except KeyError as e1:
try:
loadFromPack (rn, r["root_joint_type"], r["package"], r["urdfName"], r["urdfSuffix"], r["srdfSuffix"])
self.vf.loadUrdfInGUI ("package://" + r["package"] + "/urdf/" + r["urdfName"] + r["urdfSuffix"] + ".urdf", rn)
except KeyError as e2:
rospy.logerr ("Could not load robot " + rn + ":\n" + str(e1) + "\n" + str(e2))
# Set the root joint position in the world.
# base = self.robot.getLinkNames("root_joint")[0]
# p, q = self.tfListener.lookupTransform(self.world_frame, base, rospy.Time(0))
# self.robot.setJointPosition ("root_joint", p + q)
self.robot.rebuildRanks()
def loadEnvironment(self, param = None, xmlString = None, paramName = None, name = None):
if name is None:
name = rospy.get_param(paramName) if paramName is not None else "obstacle"
if param is not None:
xmlString = rospy.get_param(param)
if xmlString is not None:
self.manip.robot.loadEnvironmentModelFromString (xmlString, "", name)
self.vf.loadUrdfObjectsInGUI(xmlString, name)
def _createGraph(self, msg):
self.createGraph(msg.data)
def createGraph(self, paramName):
graphInfo = rospy.get_param(paramName)
grippers = rospy.get_param(paramName + "/grippers", self.ps.getAvailable("gripper"))
objects = list()
handlePerObjects = list()
contactPerObjects = list()
for objName, objDesc in graphInfo["objects"].items():
objects.append(objName)
handlePerObjects.append(objDesc["handles"])
contactPerObjects.append(objDesc["contacts"])
envNames = rospy.get_param(paramName + "/environment_contacts", self.ps.getAvailable("envcontact"))
rules = list()
for r in rospy.get_param(paramName + "/rules", list()):
checkType ("In rules, param grippers", r["grippers"], (list, tuple))
checkType ("In rules, param handles", r["handles"], (list, tuple))
checkType ("In rules, param accept", r["accept"], (bool))
rules.append(hpp.corbaserver.manipulation.Rule (r["grippers"], r["handles"], r["accept"]))
self.graph = hpp.corbaserver.manipulation.ConstraintGraph.buildGenericGraph(
self.robot,
graphInfo["name"],
grippers,
objects,
handlePerObjects,
contactPerObjects,
envNames,
rules)
def configure (self):
super(HppServerInitializer, self).configure()
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,
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}}}
# Initialization. {{{1
# Set parameters. {{{2
# robot.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold (1e-3)
# 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']
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)
ps.setParameter("ManipulationPlanner/extendStep", 0.7)
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']
q_init[rank:rank + 7] = [-4.8, -4.6, 0.91, 0, sqrt(2) / 2, sqrt(2) / 2, 0]
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 range(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 range(K):
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,
0,
0,
0,
1, # root_joint
0.0,
0.0,
-0.411354,
0.859395,
'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")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_left")
srdf_disable_collisions += srdf_disable_collisions_fmt.format("tiago/hand_safety_box", "driller/tag_support_link_top_horizontal")
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]
rank = robot.rankInConfiguration ['box/base_joint_SO3']
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)
ps.createLockedJoint (lockedJointName, j, [v,])
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.,
2,
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
-0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right
0, 0, 0, 0, 0, 0, 0, # gripper_right
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])
# 3}}}
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']
q_init [rank:rank+7] = [-4.8, -4.6, 0.91,0,sqrt(2)/2,sqrt(2)/2,0]
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']
q_init [rank] = 0.5
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.client.basic.problem.resetRoadmap ()
ps.setErrorThreshold(1e-3)
ps.setMaxIterProjection(40)
# Generate initial and goal configuration.
# 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']
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
q[9] = q[15] = -0.7
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 ()
q0 = q_init [::]
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]
rank = robot.rankInConfiguration['box/base_joint_SO3']
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}}}
# 1}}}
# Initialization. {{{1
# Set parameters. {{{2
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')
# 2}}}
# 1}}}
# Initialization. {{{1
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()
for i in range(nSphere):
vf.loadObjectModel(Sphere, 'sphere{0}'.format(i))
robot.setJointBounds('sphere{0}/root_joint'.format(i), [
-1.,
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
# q_init [-7:] = [2, 1, 0.65, 0.7071067811865476, 0, -0.7071067811865475, 0]
q_init[-7:] = [2, 1, 0.65, 0.7071067811865476, 0.5, -0.5, 0]
