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
t.translation.x = transform.translation[0] t.translation.y = transform.translation[1] t.translation.z = transform.translation[2] publisher.publish(t) client = CorbaClient() bag = rosbag.Bag("/usr/localDev/rosbag-calib/pyrene-calib.bag") Robot.packageName = "agimus_demos" Robot.urdfName = "talos" Robot.urdfSuffix = "_calibration_camera" Robot.srdfSuffix = "" robot = Robot("talos", "talos", rootJointType="freeflyer", client=client) q = robot.getCurrentConfig() pub_cMo = rospy.Publisher("/camera_object", TransformROS, queue_size=100) pub_fMe = rospy.Publisher("/world_effector", TransformROS, queue_size=100) rospy.init_node("pose_sender", anonymous=True) i = 0 for (_, joint_states, _), (_, checkerboard_pose, _) in zip(bag.read_messages(topics=["joints"]), bag.read_messages(topics=["chessboard"])): root_joint_rank = robot.rankInConfiguration["talos/root_joint"] q[root_joint_rank:root_joint_rank + 7] = [0, 0, 1, 0, 0, 0, 1] joints_name_value_tuple = zip(joint_states.name, joint_states.position)
Robot.packageName = "talos_data" Robot.urdfName = "talos" Robot.urdfSuffix = '_full' Robot.srdfSuffix= '' 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
Robot.urdfName = "talos" Robot.urdfSuffix = "_calibration_camera" Robot.srdfSuffix = "" class Mire(object): 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.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']
def resetConnection(self): self.r = Robot("", "", "", False)
from hpp.corbaserver import Client from hpp.corbaserver.manipulation import Client as ManipClient cl = Client() mcl = ManipClient() mcl.robot.create ("test") cl.robot.appendJoint ("", "A/root_joint", "planar", [0,0,0,0,0,0,1]) cl.robot.createSphere ("A/root_body", 0.001) cl.robot.addObjectToJoint ("A/root_joint", "A/root_body", [0,0,1,0,0,0,1]) mcl.robot.finishedRobot ("A") cl.robot.appendJoint ("", "B/root_joint", "planar", [0,0,0,0,0,0,1]) cl.robot.createBox ("B/root_body", 0.001, 0.002, 0.004) cl.robot.addObjectToJoint ("B/root_joint", "B/root_body", [0,0,1,0,0,0,1]) mcl.robot.finishedRobot ("B") from hpp.corbaserver.manipulation.robot import Robot robot = Robot()