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]