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 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 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))
vf.loadEnvironmentModel (Environment, "kitchen_area") robot.setJointBounds ("pr2/root_joint" , [-5,-2,-5.2,-2.7,-2,2,-2,2] ) robot.setJointBounds ("box/root_joint", [-5.1,-2,-5.2,-2.7,0,1.5,-2,2,-2,2,-2,2,-2,2]) # 2}}} # 2}}} # 1}}} # Initialization. {{{1 # Set parameters. {{{2 robot.client.basic.problem.resetRoadmap () ps.setErrorThreshold (1e-3) ps.setMaxIterProjection (40) # 2}}} # Create lists of joint names - useful to define passive joints. {{{2 jointNames = dict () jointNames['all'] = robot.getJointNames () jointNames['pr2'] = list () jointNames['allButPR2LeftArm'] = list () for n in jointNames['all']: if n.startswith ("pr2"): jointNames['pr2'].append (n) if not n.startswith ("pr2/l_gripper"): jointNames['allButPR2LeftArm'].append (n) ps.addPassiveDofs ('pr2', jointNames['pr2']) # 2}}}
joint_bounds["default"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] shrinkJointRange(robot, 0.6) joint_bounds["grasp-generation"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] setRobotJointBounds("default") shrinkJointRange(robot, 0.95) joint_bounds["planning"] = [ (jn, robot.getJointBounds(jn)) for jn in robot.jointNames ] ps.selectPathValidation("Graph-Discretized", 0.05) #ps.selectPathValidation("Graph-Dichotomy", 0) #ps.selectPathValidation("Graph-Progressive", 0.02) ps.selectPathProjector("Progressive", 0.2) ps.addPathOptimizer("EnforceTransitionSemantic") ps.addPathOptimizer("SimpleTimeParameterization") if isSimulation: ps.setMaxIterProjection (1) ps.setParameter("SimpleTimeParameterization/safety", 0.25) ps.setParameter("SimpleTimeParameterization/order", 2) ps.setParameter("SimpleTimeParameterization/maxAcceleration", 1.0) ps.setParameter("ManipulationPlanner/extendStep", 0.7) ps.setParameter("SteeringMethod/Carlike/turningRadius", 0.05) q0 = robot.getCurrentConfig() q0[:4] = [-3., 2., 0, -1] #q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.15 q0[robot.rankInConfiguration['tiago/torso_lift_joint']] = 0.34 q0[robot.rankInConfiguration['tiago/arm_1_joint']] = 0.10 q0[robot.rankInConfiguration['tiago/arm_2_joint']] = -1.47 q0[robot.rankInConfiguration['tiago/arm_3_joint']] = -0.16 q0[robot.rankInConfiguration['tiago/arm_4_joint']] = 1.87
ps.createLockedJoint("gripper_l_lock_j", "talos/gripper_left_joint", [0]) ps.createLockedJoint("gripper_l_lock_msj", "talos/gripper_left_motor_single_joint", [0]) # lock drawer closed ps.createLockedJoint("drawer_lock", "desk/upper_case_bottom_upper_drawer_bottom_joint", [0]) # static stability constraint ps.addPartialCom("Talos_CoM", ["talos/root_joint"]) ps.createStaticStabilityConstraints("talos_static_stability", q, "Talos_CoM", ProblemSolver.FIXED_ON_THE_GROUND) tpc = ps.getPartialCom("Talos_CoM") rla = robot.getJointPosition(robot.leftAnkle) com_wf = np.array(tpc) tf_la = Transform(rla) com_la = tf_la.inverse().transform(com_wf) ps.createRelativeComConstraint("talos_com_constraint_to_la", "Talos_CoM", robot.leftAnkle, com_la.tolist(), (True, True, False)) ps.setMaxIterProjection(40) # Set the maximum number of iterations of the projection algorithm. Must be called before the graph creation. # constraint graph rules = [ Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_top"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/touchpoint_right_front"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_front", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["desk/touchpoint_left_drawer", "^$"], True), Rule(["talos/left_gripper", "talos/right_gripper"], ["^$", "desk/upper_drawer_spherical_handle"], True) ] graph = ConstraintGraph(robot, "graph") factory = ConstraintGraphFactory(graph) factory.setGrippers(["talos/left_gripper", "talos/right_gripper"]) factory.setObjects(["desk"], [["desk/upper_drawer_spherical_handle", "desk/touchpoint_right_top", "desk/touchpoint_right_front", "desk/touchpoint_left_front", "desk/touchpoint_left_drawer"]], [[]])
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], )