for n in jointNames["baxterRightSide"]: ps.createLockedJoint (n, n, [0,]) for n in jointNames["baxterLeftSide"]: ps.createLockedJoint (n, n, [0,]) lockAll = lockFingers + lockHead + jointNames["baxterLeftSide"] # 4}}} # Create constraints corresponding to each object {{{4 lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock') lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock') # 4}}} # Create gaze constraints {{{4 ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) # 4}}} # 3}}} # Create the graph. {{{3 # cg.createNode (['r2', 'r1', '2on1', 'free']) cg.createNode (['r2', 'r1', 'free']) cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table']) # cg.setConstraints (node='2on1', numConstraints=['box1_on_table','box2_on_box1']) cg.setConstraints (node='r1', grasps = ['r1_grasp',]) cg.setConstraints (node='r2', grasps = ['r2_grasp',])
for n in jointNames["baxterRightSide"]: ps.createLockedJoint (n, n, [0,]) for n in jointNames["baxterLeftSide"]: ps.createLockedJoint (n, n, [0,]) lockHeadFingersAndLeftSide = lockFingers + lockHead + jointNames["baxterLeftSide"] # 4}}} # Create constraints corresponding to each object {{{4 lockBox1 = ps.lockFreeFlyerJoint ("box1/" + Box.joint, 'box1_lock') lockBox2 = ps.lockFreeFlyerJoint ("box2/" + Box.joint, 'box2_lock') # 4}}} # Create gaze constraints {{{4 ps.createPositionConstraint ("gaze1", "baxter/display_joint", "box1/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) ps.createPositionConstraint ("gaze2", "baxter/display_joint", "box2/base_joint_xyz", [0,0,0], [0,0,0], [1,0,0]) # 4}}} # 3}}} # Create the graph. {{{3 cg.createNode (['grasp2', 'grasp1', 'free']) cg.setConstraints (node='free', numConstraints=['box1_on_table','box2_on_table']) cg.setConstraints (node='grasp1', grasps = ['grasp1',]) cg.setConstraints (node='grasp2', grasps = ['grasp2',]) def genGrasp (iBox, lockBox, otherLockBox): iStr = str(iBox)
ps.addPartialCom ("talos", ["talos/root_joint"]) ps.addPartialCom ("talos_box", ["talos/root_joint", "box/root_joint"]) ps.createStaticStabilityConstraints ("balance", half_sitting, "talos", ProblemSolver.FIXED_ON_THE_GROUND) foot_placement = [ "balance/pose-left-foot", "balance/pose-right-foot" ] foot_placement_complement = [ ] robot.setCurrentConfig(half_sitting) com_wf = np.array(ps.getPartialCom("talos")) tf_la = Transform (robot.getJointPosition(robot.leftAnkle)) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint ("com_talos_box", "talos_box", robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createRelativeComConstraint ("com_talos" , "talos" , robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createPositionConstraint ("gaze", "talos/rgbd_optical_joint", "box/root_joint", (0,0,0), (0,0,0), (True, True, False)) left_gripper_lock = [] right_gripper_lock = [] other_lock = ["talos/torso_1_joint"] for n in robot.jointNames: s = robot.getJointConfigSize(n) r = robot.rankInConfiguration[n] if n.startswith ("talos/gripper_right"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) right_gripper_lock.append(n) elif n.startswith ("talos/gripper_left"): ps.createLockedJoint(n, n, half_sitting[r:r+s]) left_gripper_lock.append(n) elif n in other_lock: ps.createLockedJoint(n, n, half_sitting[r:r+s])
leftfoot = [ [ # "leftfoot/comz" , "leftfoot/com", "leftfoot/z", "leftfoot/rxry" ], [ "leftfoot/xy", ] ] # "leftfoot/rxryrz"]] ps.createRelativeComConstraint(leftfoot[0][0], "", robot.leftAnkle, (0, 0, 0.7028490491159864), (True, True, True)) ps.createPositionConstraint(leftfoot[0][1], robot.leftAnkle, "", (0, 0, 0), leftpos[0:3], (False, False, True)) ps.createPositionConstraint(leftfoot[1][0], robot.leftAnkle, "", (0, 0, 0), leftpos[0:3], (True, True, False)) ps.client.basic.problem.setConstantRightHandSide(leftfoot[1][0], False) ps.createOrientationConstraint(leftfoot[0][2], robot.leftAnkle, "", (1, 0, 0, 0), (True, True, True)) # ps.createOrientationConstraint (leftfoot[1][2], robot.leftAnkle, "", (1,0,0,0), (False, False, True)) # ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][2], True) rightfoot = [ [ # "rightfoot/comz" , "rightfoot/com", "rightfoot/z", "rightfoot/rxry" ],
lockJoint("part/root_joint", q0, "lock_part", constantRhs=False) c_lock_part = ps.hppcorba.problem.getConstraint("lock_part") for n in robot.jointNames: if n.startswith('tiago/gripper_') or n.startswith('tiago/hand_'): ljs.append(lockJoint(n, q0)) lock_arm = [ lockJoint(n, q0) for n in robot.jointNames if n.startswith("tiago/arm") or n.startswith('tiago/torso')] lock_head = [ lockJoint(n, q0) for n in robot.jointNames if n.startswith("tiago/head")] # Create "Look at gripper" constraints: for (X,Y,Z) the position of the gripper in the camera frame, # (X, Y) = 0 and Z >= 0 tool_gripper = "driller/drill_tip" ps.createPositionConstraint("look_at_gripper", "tiago/camera_color_optical_frame", tool_gripper, (0,0,0), (0,0,0), (True,True,True)) look_at_gripper = ps.hppcorba.problem.getConstraint("look_at_gripper") import hpp_idl look_at_gripper.setComparisonType([hpp_idl.hpp.EqualToZero,hpp_idl.hpp.EqualToZero,hpp_idl.hpp.Superior]) # Create "Look at part" constraint ps.createPositionConstraint("look_at_part", "tiago/camera_color_optical_frame", "part/to_tag_00001", (0,0,0), (0,0,0), (True,True,False)) look_at_part = ps.hppcorba.problem.getConstraint("look_at_part") # 3}}} # {{{3 Constraint graph instanciation from hpp.corbaserver.manipulation import ConstraintGraphFactory graph = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(graph) factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ])
for n in robot.jointNames: if n.startswith('tiago/gripper_') or n.startswith('tiago/hand_'): ljs.append(lockJoint(n, q0)) lock_arm = [ lockJoint(n, q0) for n in robot.jointNames if n.startswith("tiago/arm") or n.startswith('tiago/torso') ] lock_head = [ lockJoint(n, q0) for n in robot.jointNames if n.startswith("tiago/head") ] tool_gripper = "driller/drill_tip" ps.createPositionConstraint("look_at_gripper", "tiago/xtion_rgb_optical_frame", tool_gripper, (0, 0, 0), (0, 0, 0), (True, True, True)) look_at_gripper = ps.hppcorba.problem.getConstraint("look_at_gripper") import hpp_idl look_at_gripper.setComparisonType( [hpp_idl.hpp.EqualToZero, hpp_idl.hpp.EqualToZero, hpp_idl.hpp.Superior]) ps.createPositionConstraint("look_at_part", "tiago/xtion_rgb_optical_frame", "part/to_tag_100_base", (0, 0, 0), (0, 0, 0), (True, True, False)) look_at_part = ps.hppcorba.problem.getConstraint("look_at_part") # 3}}} # {{{3 Constraint graph instanciation from hpp.corbaserver.manipulation import ConstraintGraphFactory graph = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(graph)
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], )
# 3}}} # Generate constraints {{{3 graph = ConstraintGraph (robot, 'graph') robot.setCurrentConfig (q_init) leftpos = robot.getJointPosition (robot.leftAnkle) rightpos = robot.getJointPosition (robot.rightAnkle) leftfoot = [ [ # "leftfoot/comz" , "leftfoot/com", "leftfoot/z" , "leftfoot/rxry"], ["leftfoot/xy", ]] # "leftfoot/rxryrz"]] ps.createRelativeComConstraint (leftfoot[0][0], "", robot.leftAnkle, (0,0,0.7028490491159864), (True,True,True)) ps.createPositionConstraint (leftfoot[0][1], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (False, False, True)) ps.createPositionConstraint (leftfoot[1][0], robot.leftAnkle, "", (0,0,0), leftpos[0:3], (True, True, False)) ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][0], False) ps.createOrientationConstraint (leftfoot[0][2], robot.leftAnkle, "", (1,0,0,0), (True, True, True)) # ps.createOrientationConstraint (leftfoot[1][2], robot.leftAnkle, "", (1,0,0,0), (False, False, True)) # ps.client.basic.problem.setConstantRightHandSide (leftfoot[1][2], True) rightfoot = [ [ # "rightfoot/comz" , "rightfoot/com", "rightfoot/z" , "rightfoot/rxry"], ["rightfoot/xy", ]] # "rightfoot/rxryrz"]] ps.createRelativeComConstraint (rightfoot[0][0], "", robot.rightAnkle, (0,0,0.7028490491159864), (True,True,True)) ps.createPositionConstraint (rightfoot[0][1], robot.rightAnkle, "", (0,0,0), rightpos[0:3], (False, False, True)) ps.createPositionConstraint (rightfoot[1][0], robot.rightAnkle, "", (0,0,0), rightpos[0:3], (True, True, False))
s = robot.rankInConfiguration[jname] e = s + robot.getJointConfigSize(jname) ps.createLockedJoint(cname, jname, q[s:e]) ps.setConstantRightHandSide(cname, True) return cname ljs = list() ljs.append(lockJoint("tiago/root_joint", q0)) for n in robot.jointNames: if n.startswith('tiago/hand_'): ljs.append(lockJoint(n, q0)) ps.createPositionConstraint("gaze", "tiago/xtion_rgb_optical_frame", "driller/tag_joint", (0, 0, 0), (0, 0, 0), (True, True, False)) from hpp.corbaserver.manipulation import ConstraintGraphFactory graph = ConstraintGraph(robot, 'graph') factory = ConstraintGraphFactory(graph) factory.setGrippers([ "tiago/gripper", "driller/drill_tip", ]) factory.setObjects([ "driller", "skin", ], [ [ "driller/handle",