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 __init__(self, compositeName, robotName, load=True): Parent.__init__(self, compositeName, robotName, self.rootJointType, load) self.rightAnkle = robotName + "/leg_right_6_joint" self.leftAnkle = robotName + "/leg_left_6_joint" self.rightWrist = robotName + "/arm_right_7_joint" self.leftWrist = robotName + "/arm_left_7_joint"
def __init__ (self, compositeName, robotName, load = True, rootJointType = "planar"): Parent.__init__ (self, compositeName, robotName, rootJointType, load) self.tf_root = "base_footprint" self.halfSitting = self.getCurrentConfig () for j, v in self.halfSittingDict.iteritems (): index = self.rankInConfiguration [robotName + "/" + j] self.halfSitting [index] = v
def __init__(self, compositeName, robotName, load=True, rootJointType="planar", **kwargs): Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs)
def __init__(self, compositeName, robotName, load=True, rootJointType="anchor", **kwargs): Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs) self.tf_root = "base_footprint"
def __init__(self, compositeName, robotName, load=True, rootJointType=rootJointType): Parent.__init__(self, compositeName, robotName, rootJointType, load) self.rightWrist = "wrist_3_joint" self.leftWrist = "wrist_3_joint" self.endEffector = "ee_fixed_joint"
def __init__ (self, compositeName, robotName): Parent.__init__ (self, compositeName, robotName, self.rootJointType) hs = self.halfSitting.copy () for joint, value in hs.iteritems (): self.halfSitting [self.displayName + "/" + joint] = value self.rightWrist = self.displayName + "/RARM_JOINT5" self.leftWrist = self.displayName + "/LARM_JOINT5" self.rightAnkle = self.displayName + "/RLEG_JOINT5" self.leftAnkle = self.displayName + "/LLEG_JOINT5" self.waist = self.displayName + "/base_joint_SO3" self.chest = self.displayName + "/CHEST_JOINT1" self.gaze = self.displayName + "/HEAD_JOINT1"
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 __init__( self, compositeName, robotName, load=True, rootJointType="planar", **kwargs ): """ Constructor \\param compositeName name of the composite robot that will be built later, \\param robotName name of the first robot that is loaded now, \\param load whether to actually load urdf files. Set to no if you only want to initialize a corba client to an already initialized problem. \\param rootJointType type of root joint among ("freeflyer", "planar", "anchor"), """ Parent.__init__(self, compositeName, robotName, rootJointType, load, **kwargs)
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']
# / import numpy as np from hpp import Transform from hpp.corbaserver.manipulation import ConstraintGraph, ProblemSolver, newProblem from hpp.corbaserver.manipulation.robot import Robot from hpp.gepetto.manipulation import ViewerFactory newProblem() Robot.packageName = "talos_data" Robot.urdfName = "talos" Robot.urdfSuffix = "_full" Robot.srdfSuffix = "" 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.selectPathProjector("Progressive", 0.2) ps.setErrorThreshold(1e-3) ps.setMaxIterProjection(40) ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps) half_sitting = [ 0,
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], )
def resetConnection(self): self.r = Robot("", "", "", False)
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
def __init__ (self, compositeName, robotName, load = True, rootJointType = rootJointType): Parent.__init__ (self, compositeName, robotName, rootJointType, load) self.rightWrist = "wrist_3_joint" self.leftWrist = "wrist_3_joint" self.endEffector = "ee_fixed_joint"
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']
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)
def __init__ (self, compositeName, robotName, load = True, rootJointType = "planar", **kwargs): Parent.__init__ (self, compositeName, robotName, rootJointType, load, **kwargs)
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()
def __init__ (self, compositeName, robotName, load = True, rootJointType = "planar"): Parent.__init__ (self, compositeName, robotName, rootJointType, load) self.tf_root = "base_footprint"
srdfSuffix = "" pose = "pose" handles = [] contacts = [ "top", ] newProblem() Robot.packageName = 'talos_data' Robot.urdfName = 'talos' Robot.urdfSuffix = '_full_v2' Robot.srdfSuffix = '' 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("Graph-RandomShortcut") ps.addPathOptimizer("SimpleTimeParameterization") vf = ViewerFactory(ps)
def __init__ (self, compositeName, robotName, load = True, rootJointType = "anchor"): Parent.__init__ (self, compositeName, robotName, rootJointType, load) self.tf_root = "base_footprint"
def __init__(self, agentName): print 'initialising a HyQ agent' Robot.__init__(self, 'meta', agentName, "planar")