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], )
"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"]]
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)) ps.client.basic.problem.setConstantRightHandSide (rightfoot[1][0], False) ps.createOrientationConstraint (rightfoot[0][2], robot.rightAnkle, "", (1,0,0,0), (True, True, True)) # ps.createOrientationConstraint (rightfoot[1][2], robot.rightAnkle, "", (1,0,0,0), (False, False, True))