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 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 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(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 = "" 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.,