def createStaticStabilityConstraint(self, prefix, comName, leftAnkle, rightAnkle, q0): robot = self.hppcorba.robot problem = self.hppcorba.problem _tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle)) Ml = Transform(_tfs[0]) Mr = Transform(_tfs[1]) self.setCurrentConfig(q0) x = self._getCOM(comName) result = [] # COM wrt left ankle frame xloc = Ml.inverse().transform(x) result.append(prefix + "relative-com") problem.createRelativeComConstraint(result[-1], comName, leftAnkle, xloc.tolist(), (True, ) * 3) # Pose of the left foot result.append(prefix + "pose-left-foot") problem.createTransformationConstraint2( result[-1], "", leftAnkle, Ml.toTuple(), (0, 0, 0, 0, 0, 0, 1), (True, True, True, True, True, True)) # Pose of the right foot result.append(prefix + "pose-right-foot") problem.createTransformationConstraint2( result[-1], "", rightAnkle, Mr.toTuple(), (0, 0, 0, 0, 0, 0, 1), (True, True, True, True, True, True)) return result
def createGraspConstraint(gripperName, handleName): name = "Relative transformation " + gripperName + "/" + handleName gjn, gpos = robot.getGripperPositionInJoint(gripperName) hjn, hpos = robot.getHandlePositionInJoint(handleName) ps.createTransformationConstraint( name, gjn, hjn, (Transform(gpos) * Transform(hpos).inverse()).toTuple(), [True] * 6) return name
def validateGazeConstraint(ps, q, whichArm): robot = ps.robot robot.setCurrentConfig(q) Mcamera = Transform(robot.getLinkPosition("talos/rgbd_optical_frame")) Mtarget = Transform( robot.getLinkPosition("talos/arm_" + whichArm + "_7_link")) z = (Mcamera.inverse() * Mtarget).translation[2] if z < .1: return False return True
def addAlignmentConstrainttoEdge(ps, handle, graph): #recover id of handle handleId = all_handles.index(handle) J1, gripperPose = ps.robot.getGripperPositionInJoint(tool_gripper) J2, handlePose = ps.robot.getHandlePositionInJoint(handle) T1 = Transform(gripperPose) T2 = Transform(handlePose) constraintName = handle + '/alignment' ps.client.basic.problem.createTransformationConstraint2\ (constraintName, J1, J2, T1.toTuple(), T2.toTuple(), [False, True, True, False, False, False]) # Set constraint edgeName = tool_gripper + ' > ' + handle + ' | 0-0_12' graph.addConstraints(edge = edgeName, constraints = \ Constraints(numConstraints=[constraintName])) edgeName = tool_gripper + ' < ' + handle + ' | 0-0:1-{}_21'.format(handleId) graph.addConstraints(edge = edgeName, constraints = \ Constraints(numConstraints=[constraintName]))
def setSotFrameFromHpp(self, pinrobot): # The joint should be available in the robot model used by SOT n = self.hppjoint self.sotpose = self.hpppose self.robotname, self.sotjoint = parseHppName (n) while self.sotjoint not in pinrobot.names: self.sotpose = Transform(self.hpp.basic.robot.getJointPositionInParentFrame(n)) * self.sotpose n = self.hpp.basic.robot.getParentJointName(n) robotname, self.sotjoint = parseHppName (n)
def createAlignedCOMStabilityConstraint(self, prefix, comName, leftAnkle, rightAnkle, q0, sliding): robot = self.hppcorba.robot problem = self.hppcorba.problem _tfs = robot.getJointsPosition(q0, (leftAnkle, rightAnkle)) Ml = Transform(_tfs[0]) Mr = Transform(_tfs[1]) robot.setCurrentConfig(q0) x = self._getCOM(robot, comName) result = [] # COM between feet result.append(prefix + "com-between-feet") problem.createComBetweenFeet( result[-1], comName, leftAnkle, rightAnkle, (0, 0, 0), (0, 0, 0), "", x.tolist(), (True, ) * 4, ) if sliding: mask = (False, False, True, True, True, False) else: mask = (True, ) * 6 # Pose of the right foot result.append(prefix + "pose-right-foot") problem.createTransformationConstraint2(result[-1], "", rightAnkle, Mr.toTuple(), (0, 0, 0, 0, 0, 0, 1), mask) # Pose of the left foot result.append(prefix + "pose-left-foot") problem.createTransformationConstraint2(result[-1], "", leftAnkle, Ml.toTuple(), (0, 0, 0, 0, 0, 0, 1), mask) return result
def selected(self, event): if event.hasIntersection(): self.pointLabel["local"].text = vec2str(vec(event.point(True))) self.normalLabel["local"].text = vec2str(vec(event.normal(True))) self.pointLabel["global"].text = vec2str(vec(event.point(False))) self.normalLabel["global"].text = vec2str(vec(event.normal(False))) if len(self.refName.text) == 0: print self.refName.text else: T = Transform( self.plugin.client.robot.getJointPosition( str(self.refName.text))).inverse() try: self.pointLabel["reference"].text = vec2str( T.transform(vec(event.point(False)))) self.normalLabel["reference"].text = vec2str( T.quaternion.transform(vec(event.normal(False)))) except ValueError as e: print e print event.point(False) print vec(event.point(False)) event.done()
0, 0, # head 0,0,0,0,0,0,1, # box ] q_init = robot.getCurrentConfig() 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])
## Project initial configuration on state 'placement' res, q_init, error = graph.applyNodeConstraints ('placement', q1) q2 = q1 [::] q2 [7] = .2 ## Project goal configuration on state 'placement' res, q_goal, error = graph.applyNodeConstraints ('placement', q2) ## Define manipulation planning problem ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) # v = vf.createViewer () # pp = PathPlayer (v) # v (q1) ## Build relative position of the ball with respect to the gripper for i in range (100): q = robot.shootRandomConfig () res,q3,err = graph.generateTargetConfig ('grasp-ball', q_init, q) if res and robot.isConfigValid (q3): break; if res: robot.setCurrentConfig (q3) gripperPose = Transform (robot.getJointPosition (gripperName)) ballPose = Transform (robot.getJointPosition (ballName)) gripperGraspsBall = gripperPose.inverse () * ballPose gripperAboveBall = Transform (gripperGraspsBall) gripperAboveBall.translation [2] += .1
def getObjectPosition(self, objectName): return Transform(self.hppcorba.robot.getObjectPosition(objectName))
def setHppGripper (self, name): self.name = name # Get parent joint and position from HPP self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getGripperPositionInJoint(name) self.hpppose = Transform (self.hpppose)
p.appendPath(q) return p # 2}}} # {{{2 Problem resolution # {{{3 Finalize FOV filter res, q, err = graph.applyNodeConstraints(free, q0) assert res robot.setCurrentConfig(q) oMh, oMd = robot.hppcorba.robot.getJointsPosition( q, ["tiago/hand_tool_link", "driller/base_link"]) tiago_fov.appendUrdfModel(Driller.urdfFilename, "hand_tool_link", (Transform(oMh).inverse() * Transform(oMd)).toTuple(), prefix="driller/") # 3}}} # {{{3 Create InStatePlanner armPlanner = InStatePlanner() armPlanner.setEdge(loop_free) # Set collision margin between mobile base and the rest because the collision model is not correct. bodies = ("tiago/torso_fixed_link_0", "tiago/base_link_0") cfgVal = wd(armPlanner.cproblem.getConfigValidations()) pathVal = wd(armPlanner.cproblem.getPathValidation()) for _, la, lb, _, _ in zip(*robot.distancesToCollision()): if la in bodies or lb in bodies: cfgVal.setSecurityMarginBetweenBodies(la, lb, 0.07) pathVal.setSecurityMarginBetweenBodies(la, lb, 0.07)
return coord[0:2, 0] # END INIT CHESSBOARD AND CAMERA PARAM AND FUNCTIONS ps.addPartialCom("talos", ["talos/root_joint"]) ps.addPartialCom("talos_mire", ["talos/root_joint", "mire/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_mire", "talos_mire", robot.leftAnkle, com_la.tolist(), (True, True, True)) ps.createRelativeComConstraint("com_talos", "talos", robot.leftAnkle, com_la.tolist(), (True, True, True)) left_gripper_lock = [] right_gripper_lock = [] head_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("gripper_l_lock_f1j", "talos/gripper_left_fingertip_1_joint", [0]) ps.createLockedJoint("gripper_l_lock_f2j", "talos/gripper_left_fingertip_2_joint", [0]) ps.createLockedJoint("gripper_l_lock_isj", "talos/gripper_left_inner_single_joint", [0]) ps.createLockedJoint("gripper_l_lock_f3j", "talos/gripper_left_fingertip_3_joint", [0]) 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) ]
## Project initial configuration on state 'placement' res, q_init, error = ps.client.manipulation.problem.applyConstraints \ (graph.nodes ['placement'], q1) q2 = q1[::] q2[7] = .2 ## Project goal configuration on state 'placement' res, q_goal, error = ps.client.manipulation.problem.applyConstraints \ (graph.nodes ['placement'], q2) ## Define manipulation planning problem ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) ps.selectPathValidation("Dichotomy", 0) pp = PathPlayer(ps.client.basic, r) ## Build relative position of the ball with respect to the gripper for i in range(100): q = robot.shootRandomConfig() res, q3, err = graph.generateTargetConfig('grasp-ball', q_init, q) if res and robot.isConfigValid(q3): break if res: robot.setCurrentConfig(q3) gripperPose = Transform(robot.getLinkPosition(gripperName)) ballPose = Transform(robot.getLinkPosition(ballName)) gripperAboveBall = gripperPose.inverse() * ballPose gripperAboveBall.translation[2] += .1
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) for name, value in joints_name_value_tuple: joint_name = "talos/" + name q[robot.rankInConfiguration[joint_name]] = value robot.setCurrentConfig(q) gripper = Transform( robot.getJointPosition("talos/gripper_left_base_link_joint")) camera = Transform(robot.getJointPosition("talos/rgbd_rgb_optical_joint")) fMe = gripper.inverse() * camera cMo = Transform([ checkerboard_pose.pose.position.x, checkerboard_pose.pose.position.y, checkerboard_pose.pose.position.z, checkerboard_pose.pose.orientation.x, checkerboard_pose.pose.orientation.y, checkerboard_pose.pose.orientation.z, checkerboard_pose.pose.orientation.w, ]) publishTransform(pub_cMo, cMo) publishTransform(pub_fMe, fMe)
def getObjectPosition(self, objectName): return Transform(self.client.basic.robot.getObjectPosition(objectName))
if len(paths) == 0: return None p = paths[0].asVector() for q in paths[1:]: p.appendPath(q) return p # 2}}} # {{{2 Problem resolution # {{{3 Finalize FOV filter res, q, err = graph.applyNodeConstraints(free, q0) assert res robot.setCurrentConfig(q) oMh, oMd = robot.hppcorba.robot.getJointsPosition(q, ["tiago/hand_tool_link", "driller/base_link"]) tiago_fov.appendUrdfModel(Driller.urdfFilename, "hand_tool_link", (Transform(oMh).inverse() * Transform(oMd)).toTuple(), prefix="driller/") # 3}}} # {{{3 Create InStatePlanner armPlanner = InStatePlanner () armPlanner.setEdge(loop_free) #armPlanner.optimizerTypes = [ "SplineGradientBased_bezier3", ] armPlanner.optimizerTypes = [ ] armPlanner.cproblem.setParameter("SimpleTimeParameterization/safety", Any(TC_float, 0.25)) armPlanner.cproblem.setParameter("SimpleTimeParameterization/order", Any(TC_long, 2)) armPlanner.cproblem.setParameter("SimpleTimeParameterization/maxAcceleration", Any(TC_float, 1.0)) armPlanner.maxIterPathPlanning = 600 armPlanner.timeOutPathPlanning = 10. # Set collision margin between mobile base and the rest because the collision model is not correct. bodies = ("tiago/torso_fixed_link_0", "tiago/base_link_0")