def setUp(self): # create a action client of move group self._move_client = SimpleActionClient('move_group', MoveGroupAction) self._move_client.wait_for_server() moveit_commander.roscpp_initialize(sys.argv) group_name = moveit_commander.RobotCommander().get_group_names()[0] group = moveit_commander.MoveGroupCommander(group_name) # prepare a joint goal self._goal = MoveGroupGoal() self._goal.request.group_name = group_name self._goal.request.max_velocity_scaling_factor = 0.1 self._goal.request.max_acceleration_scaling_factor = 0.1 self._goal.request.start_state.is_diff = True goal_constraint = Constraints() joint_values = [0.1, 0.2, 0.3, 0.4, 0.5, 0.6] joint_names = group.get_active_joints() for i in range(len(joint_names)): joint_constraint = JointConstraint() joint_constraint.joint_name = joint_names[i] joint_constraint.position = joint_values[i] joint_constraint.weight = 1.0 goal_constraint.joint_constraints.append(joint_constraint) self._goal.request.goal_constraints.append(goal_constraint) self._goal.planning_options.planning_scene_diff.robot_state.is_diff = True
def execute(self, userdata): header = Header() header.frame_id = "base_link" header.stamp = rospy.Time.now() userdata.move_it_joint_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(userdata.manip_joint_names, userdata.manip_goal_joint_pose): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) userdata.move_it_joint_goal.request.goal_constraints.append(goal_c) userdata.move_it_joint_goal.request.num_planning_attempts = 5 userdata.move_it_joint_goal.request.allowed_planning_time = 5.0 userdata.move_it_joint_goal.planning_options.plan_only = False # False = Plan + Execute ; True = Plan only userdata.move_it_joint_goal.planning_options.planning_scene_diff.is_diff = True userdata.move_it_joint_goal.request.group_name = userdata.manip_joint_group rospy.loginfo("Group Name: " + userdata.manip_joint_group) rospy.loginfo("Joints name: " + str(userdata.manip_joint_names)) rospy.loginfo("Joints Values: " + str(userdata.manip_goal_joint_pose)) rospy.loginfo("GOAL TO SEND IS:... " + str(userdata.move_it_joint_goal)) return "succeeded"
def create_move_group_joints_goal(joint_names, joint_values, group="right_arm_torso", plan_only=True): """ Creates a move_group goal based on pose. @arg joint_names list of strings of the joint names @arg joint_values list of digits with the joint values @arg group string representing the move_group group to use @arg plan_only bool to for only planning or planning and executing @return MoveGroupGoal with the desired contents""" header = Header() header.frame_id = 'base_link' header.stamp = rospy.Time.now() moveit_goal = MoveGroupGoal() goal_c = Constraints() for name, value in zip(joint_names, joint_values): joint_c = JointConstraint() joint_c.joint_name = name joint_c.position = value joint_c.tolerance_above = 0.01 joint_c.tolerance_below = 0.01 joint_c.weight = 1.0 goal_c.joint_constraints.append(joint_c) moveit_goal.request.goal_constraints.append(goal_c) moveit_goal.request.num_planning_attempts = 1 moveit_goal.request.allowed_planning_time = 5.0 moveit_goal.planning_options.plan_only = plan_only moveit_goal.planning_options.planning_scene_diff.is_diff = True moveit_goal.request.group_name = group return moveit_goal
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name): """ Given a start and goal point, returns the planned path. Args: start_point (list of float): A starting joint configuration. goal_point (list of float): A goal joint configuration. planner_number (int): The index of the planner to be used as returned by acquire_planner(). joint_names (list of str): The name of the joints corresponding to start_point and goal_point. group_name (str): The name of the group to which the joint names correspond. planning_time (float): Maximum allowed time for planning, in seconds. planner_config_name (str): Type of planner to use. Return: list of list of float: A sequence of points representing the joint configurations at each point on the path. """ planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan) rospy.loginfo("Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" % (self.planners[planner_number], start_point, goal_point)) # Put together the service request. req = GetMotionPlanRequest() req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime() req.motion_plan_request.group_name = group_name req.motion_plan_request.num_planning_attempts = 1 req.motion_plan_request.allowed_planning_time = planning_time req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime() req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_point req.motion_plan_request.goal_constraints.append(Constraints()) req.motion_plan_request.goal_constraints[0].joint_constraints = [] for i in xrange(len(joint_names)): temp_constraint = JointConstraint() temp_constraint.joint_name = joint_names[i] temp_constraint.position = goal_point[i] temp_constraint.tolerance_above = 0.05; temp_constraint.tolerance_below = 0.05; req.motion_plan_request.goal_constraints[0].joint_constraints.append(temp_constraint) #call the planner rospy.wait_for_service(self.planners[planner_number]) rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name) try: response = planner_client(req) except rospy.ServiceException, e: rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s"%e) return None
def createJointConstraints(pose_from_params, tolerances=0.1): """Create a JointConstraints message with its joint names and positions with some default tolerances @param pose_from_params dictionary with names of the joints and it's values @param tolerances the tolerance in radians for the joint positions, defaults to 0.1 @return moveit_msgs/JointConstraints[] message with the joint names and positions""" joint_constraints = [] for joint in pose_from_params: joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = pose_from_params[joint] joint_constraint.tolerance_above = tolerances joint_constraint.tolerance_below = tolerances joint_constraint.weight = 1.0 joint_constraints.append(joint_constraint) return joint_constraints
def createJointConstraintsZero(): joint_positions = [1.7567944054, 1.68571255762, -0.35731008621, 1.06480870567, 1.36986326531, -0.662985424101, -1.31376998814] joint_constraints = [] for joint_num in range(1,8): joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_right_' + str(joint_num) + '_joint' joint_constraint.tolerance_above = 0.1 joint_constraint.tolerance_below = 0.1 joint_constraint.weight = 1.0 joint_constraint.position = 0.0 joint_constraints.append(joint_constraint) print str(joint_constraint) return joint_constraints
def createJointConstraints(joint_names, joint_positions, tolerances=0.1): """Create a JointConstraints message with its joint names and positions with some default tolerances @param joint_names names of the joints which will reference to values in param joint_positions @param joint_positions values for the joints specified in joint_names @param tolerances the tolerance in radians for the joint positions, defaults to 0.1 @return moveit_msgs/JointConstraints[] message with the joint names and positions""" joint_constraints = [] for joint, pos in zip(joint_names, joint_positions): joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = pos joint_constraint.tolerance_above = tolerances joint_constraint.tolerance_below = tolerances joint_constraint.weight = 1.0 joint_constraints.append(joint_constraint) return joint_constraints
def quick_motion_test_client(): rospy.wait_for_service('/lightning/lightning_get_path') print "Working on it..." req = GetMotionPlanRequest() req.motion_plan_request.group_name = "torso" req.motion_plan_request.start_state.joint_state.name = ["torso_lift_joint"] req.motion_plan_request.start_state.joint_state.position = [float(0.2)] tempConstraint = JointConstraint() tempConstraint.joint_name = req.motion_plan_request.start_state.joint_state.name[0] tempConstraint.position = float(0.15) fullConstraint = Constraints() fullConstraint.joint_constraints = [tempConstraint] req.motion_plan_request.goal_constraints = [fullConstraint] req.motion_plan_request.allowed_planning_time = 50.0 try: client_func = rospy.ServiceProxy('/lightning/lightning_get_path', GetMotionPlan) res = client_func(req) except rospy.ServiceException, e: print "Service call failed: %s"%e
def both_arms_go_to_pose_goal(self): # 设置动作对象变量,此处为both_arms both_arms = self.both_arms # 获取当前各轴转角 axis_angle = both_arms.get_current_joint_values() # print axis_angle # 获取当前末端执行器位置姿态 right_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_r_finger_r") left_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_l_finger_r") print right_pose_goal # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = right_pose_goal.pose.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.6 right_orientation_const.absolute_y_axis_tolerance = 0.6 right_orientation_const.absolute_z_axis_tolerance = 0.6 right_orientation_const.weight = 1 left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = left_pose_goal.pose.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.6 left_orientation_const.absolute_y_axis_tolerance = 0.6 left_orientation_const.absolute_z_axis_tolerance = 0.6 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const, left_joint_const] # consts.orientation_constraints = [right_orientation_const, left_orientation_const] both_arms.set_path_constraints(consts) # # 设置动作对象目标位置姿态 # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = Neurondata[5] # right_pose_goal.pose.position.y = Neurondata[3] # right_pose_goal.pose.position.z = Neurondata[4] # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = Neurondata[11] # left_pose_goal.pose.position.y = Neurondata[9] # left_pose_goal.pose.position.z = Neurondata[10] # # 右臂 # right_pose_goal.pose.orientation.x = Right_Qux # right_pose_goal.pose.orientation.y = Right_Quy # right_pose_goal.pose.orientation.z = Right_Quz # right_pose_goal.pose.orientation.w = Right_Quw # right_pose_goal.pose.position.x = (1266/740)*(Neurondata[5]+0.28)-0.495 # right_pose_goal.pose.position.y = (1295/780)*(Neurondata[3]+0.56)-0.754 # right_pose_goal.pose.position.z = (1355/776)*(Neurondata[4]-0.054)-0.184 # # 左臂 # left_pose_goal.pose.orientation.x = Left_Qux # left_pose_goal.pose.orientation.y = Left_Quy # left_pose_goal.pose.orientation.z = Left_Quz # left_pose_goal.pose.orientation.w = Left_Quw # left_pose_goal.pose.position.x = (1266/850)*(Neurondata[11]+0.33)-0.495 # left_pose_goal.pose.position.y = (1295/720)*(Neurondata[9]+0.19)-0.541 # left_pose_goal.pose.position.z = (1355/745)*(Neurondata[10]-0.055)-0.184 # 右臂 right_pose_goal.pose.orientation.x = Right_Qux right_pose_goal.pose.orientation.y = Right_Quy right_pose_goal.pose.orientation.z = Right_Quz right_pose_goal.pose.orientation.w = Right_Quw right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 # 左臂 left_pose_goal.pose.orientation.x = Left_Qux left_pose_goal.pose.orientation.y = Left_Quy left_pose_goal.pose.orientation.z = Left_Quz left_pose_goal.pose.orientation.w = Left_Quw left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 # # 右臂 # right_pose_goal.pose.orientation.x = right_pose_goal.pose.orientation.x # right_pose_goal.pose.orientation.y = right_pose_goal.pose.orientation.y # right_pose_goal.pose.orientation.z = right_pose_goal.pose.orientation.z # right_pose_goal.pose.orientation.w = right_pose_goal.pose.orientation.w # right_pose_goal.pose.position.x = right_pose_goal.pose.position.x # right_pose_goal.pose.position.y = right_pose_goal.pose.position.y # right_pose_goal.pose.position.z = right_pose_goal.pose.position.z # # 左臂 # left_pose_goal.pose.orientation.x = left_pose_goal.pose.orientation.x # left_pose_goal.pose.orientation.y = left_pose_goal.pose.orientation.y # left_pose_goal.pose.orientation.z = left_pose_goal.pose.orientation.z # left_pose_goal.pose.orientation.w = left_pose_goal.pose.orientation.w # left_pose_goal.pose.position.x = left_pose_goal.pose.position.x # left_pose_goal.pose.position.y = left_pose_goal.pose.position.y # left_pose_goal.pose.position.z = left_pose_goal.pose.position.z # 设置动作组的两个目标点 both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r") both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r") # 规划和输出动作 traj = both_arms.plan() both_arms.execute(traj, wait=False) # # 清除路径约束 both_arms.clear_path_constraints() # 确保输出停止 both_arms.stop()
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_finger_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] # consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Right_Qux pose_goal.orientation.y = Right_Quy pose_goal.orientation.z = Right_Quz pose_goal.orientation.w = Right_Quw pose_goal.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 pose_goal.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = 0.1644 # pose_goal.orientation.y = 0.3111 # pose_goal.orientation.z = 0.9086 # pose_goal.orientation.w = 0.2247 # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
def update(self): self.logger.debug("{0}.update()".format(self.__class__.__name__)) if not self.action_client: self.feedback_message = "no action client, did you call setup() on your tree?" return py_trees.Status.INVALID # pity there is no 'is_connected' api like there is for c++ if not self.sent_goal: if self.blackboard.frame_id != self.blackboard.target: print("Detected fail!") return py_trees.Status.FAILURE #update goal data from blackboard self.action_goal.target_pose.header.frame_id = "base_footprint" # self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x # self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y # self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x + self.x_offset self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y + self.y_offset self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z + self.z_offset # self.action_goal.target_pose.pose.position.x = 0.6 # self.action_goal.target_pose.pose.position.y = 0 # self.action_goal.target_pose.pose.position.z = 0.8 # if self.x_offset != 0: # self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x - (self.blackboard.object_pose.pose.position.x/8.0) # self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y - (self.blackboard.object_pose.pose.position.y/8.0) # self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x + 0.04 # self.action_goal.target_pose.pose.position.y = self.blackboard.object_pose.pose.position.y # self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z - 0.02 if self.mode == "put": self.action_goal.target_pose.pose.position.x = self.blackboard.object_pose.pose.position.x self.action_goal.target_pose.pose.position.z = self.blackboard.object_pose.pose.position.z + 0.1 self.action_goal.target_pose.pose.position.y = -self.blackboard.object_pose.pose.position.y theta = math.atan2(self.action_goal.target_pose.pose.position.y, self.action_goal.target_pose.pose.position.x) quat = quaternion_from_euler(0.0, 0.0, theta) self.action_goal.target_pose.pose.orientation.x = quat[0] self.action_goal.target_pose.pose.orientation.y = quat[1] self.action_goal.target_pose.pose.orientation.z = quat[2] self.action_goal.target_pose.pose.orientation.w = quat[3] # self.action_goal.target_pose.pose.orientation.x = 0 # self.action_goal.target_pose.pose.orientation.y = 0 # self.action_goal.target_pose.pose.orientation.z = 0 # self.action_goal.target_pose.pose.orientation.w = 1.0 if self.constraint: if len(self.joint) == 0: print("Constraint joint not defined.") return py_trees.Status.FAILURE for joint in self.joint: joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = self.joint[ joint_constraint.joint_name][0] joint_constraint.tolerance_above = self.joint[ joint_constraint.joint_name][1] joint_constraint.tolerance_below = self.joint[ joint_constraint.joint_name][2] joint_constraint.weight = 1.0 self.action_goal.joint_constraints.append(joint_constraint) print("Joint constrained : " + str(joint_constraint)) print("Goal position : (" + str(self.action_goal.target_pose.pose.position.x) + " , " + str(self.action_goal.target_pose.pose.position.y) + " , " + str(self.action_goal.target_pose.pose.position.z) + ")") print("Goal orientation : ", quat) self.action_client.send_goal(self.action_goal) self.sent_goal = True self.feedback_message = "sent goal to the action server" return py_trees.Status.RUNNING # print("self.action_client.get_state() : " + str(self.action_client.get_state())) # print("self.feedback_message : " + str(self.feedback_message)) self.feedback_message = self.action_client.get_goal_status_text() # Failure case if self.action_client.get_state() in [ actionlib_msgs.GoalStatus.ABORTED, actionlib_msgs.GoalStatus.PREEMPTED ]: return py_trees.Status.FAILURE result = self.action_client.get_result() if result: return py_trees.Status.SUCCESS else: self.feedback_message = self.override_feedback_message_on_running return py_trees.Status.RUNNING
def both_arms_go_to_pose_goal(self): # 设置动作对象变量,此处为both_arms both_arms = self.both_arms # 获取当前各轴转角 axis_angle = both_arms.get_current_joint_values() # print axis_angle # 获取当前末端执行器位置姿态 right_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_r_finger_r") left_pose_goal = both_arms.get_current_pose( end_effector_link="gripper_l_finger_r") print right_pose_goal # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" if Rightfinger > -55: right_joint_const.position = 0.024 else: right_joint_const.position = 0 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 # 添加右臂末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = right_pose_goal.pose.orientation right_orientation_const.link_name = "gripper_r_joint_r" right_orientation_const.absolute_x_axis_tolerance = 0.6 right_orientation_const.absolute_y_axis_tolerance = 0.6 right_orientation_const.absolute_z_axis_tolerance = 0.6 right_orientation_const.weight = 1 # 添加左臂末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = left_pose_goal.pose.orientation left_orientation_const.link_name = "gripper_l_joint_r" left_orientation_const.absolute_x_axis_tolerance = 0.6 left_orientation_const.absolute_y_axis_tolerance = 0.6 left_orientation_const.absolute_z_axis_tolerance = 0.6 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const, left_joint_const] # consts.orientation_constraints = [right_orientation_const, left_orientation_const] both_arms.set_path_constraints(consts) # 右臂目标位姿设置 right_pose_goal.pose.orientation.x = Right_Qux right_pose_goal.pose.orientation.y = Right_Quy right_pose_goal.pose.orientation.z = Right_Quz right_pose_goal.pose.orientation.w = Right_Quw right_pose_goal.pose.position.x = (Neurondata[5] - 0.05) * 1.48 + 0.053 right_pose_goal.pose.position.y = (Neurondata[3] + 0.18) * 1.48 - 0.12 right_pose_goal.pose.position.z = (Neurondata[4] - 0.53) * 1.48 + 0.47 # 左臂目标位姿设置 left_pose_goal.pose.orientation.x = Left_Qux left_pose_goal.pose.orientation.y = Left_Quy left_pose_goal.pose.orientation.z = Left_Quz left_pose_goal.pose.orientation.w = Left_Quw left_pose_goal.pose.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 left_pose_goal.pose.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 left_pose_goal.pose.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 # 设置动作组的两个目标点 both_arms.set_pose_target(right_pose_goal, end_effector_link="gripper_r_finger_r") both_arms.set_pose_target(left_pose_goal, end_effector_link="gripper_l_finger_r") # 规划和输出动作 traj = both_arms.plan() both_arms.execute(traj, wait=False) # # 清除路径约束 both_arms.clear_path_constraints() # 确保输出停止 both_arms.stop()
def main(): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = ['head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2'] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] goal.request.start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print 'Enter start state:' try: for i in range(len(joint_names)): val = float(raw_input(joint_names[i]+':')) goal.request.start_state.joint_state.position[i] = val except: print 'Using defaults for remaining joints' print goal.request.start_state.joint_state.position #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] print 'Enter goal state:' try: for i in range(len(target_joint_angles)): val = float(raw_input(joint_names[i]+':')) target_joint_angles[i] = val except: print 'Using defaults for remaining joints' print target_joint_angles tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append(Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def main(): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = [ 'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names #YOUR CODE HERE #Have a user input a specified start position for the first four angles #raw_input() might be useful to look at here goal.request.start_state.joint_state.position = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] #YOUR CODE HERE #Have a user input a specified target position for the first four angles #raw_input() might be useful to look at here TargetAnglesIn = raw_input("Please enter the first 4 target angles") temp = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] userTargetJointAngles = TargetAnglesIn.split(" ") for i in range(len(userTargetJointAngles)): userTargetJointAngles[i] = float(userTargetJointAngles[i]) userTargetJointAngles.extend(temp) tolerance = 0.0001 consts = [] print(userTargetJointAngles) for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = userTargetJointAngles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append( Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
# Get instance from moveit_commander robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() # Get group_commander from MoveGroupCommander group_name = "arm" move_group = moveit_commander.MoveGroupCommander(group_name) # Move using target_pose with constraint contraints = Constraints() contraints.name = "constraints1" jc1 = JointConstraint() jc1.joint_name = "joint2" jc1.position = 0.0 jc1.tolerance_above = 0.01 jc1.tolerance_below = 0.01 contraints.joint_constraints.append(jc1) move_group.set_path_constraints(contraints) # Move using target_pose x = 0.185 y = 0 z = 0.365 test = [x, y, z] move_group.set_position_target(test) plan = move_group.go(wait=True)
def inverse_kinematics(): # Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 50 request.ik_request.pose_stamped.header.frame_id = "base" # Create joint constraints #This is joint constraint will need to be set at the group constraints = Constraints() joint_constr = JointConstraint() joint_constr.joint_name = "right_j0" joint_constr.position = TORSO_POSITION joint_constr.tolerance_above = TOLERANCE joint_constr.tolerance_below = TOLERANCE joint_constr.weight = 0.5 constraints.joint_constraints.append(joint_constr) # Get the transformed AR Tag (x,y,z) coordinates # Only care about the x coordinate of AR tag; tells use # how far away wall is x_coord = board_x y_coord = board_y z_coord = board_z y_bias = raw_input("Input y coordinate: ") z_bias = raw_input("Input z coordinate: ") y_coord += float(y_bias) z_coord += float(z_bias) #Creating Path Planning waypoints = [] target_pose = Pose() target_pose.position.x = float(x_coord - PLANNING_BIAS) target_pose.position.y = float(y_coord) target_pose.position.z = float(z_coord) target_pose.orientation.y = 1.0/2**(1/2.0) target_pose.orientation.w = 1.0/2**(1/2.0) waypoints.append(target_pose) #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose = target_pose try: #Send the request to the service response = compute_ik(request) group = MoveGroupCommander("right_arm") #Creating a Robot Trajectory for the Path Planning jump_thres = 0.0 eef_step = 0.01 path,fraction = group.compute_cartesian_path(waypoints, eef_step, jump_thres) print("Path fraction: {}".format(fraction)) #Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) #Setting the Joint constraint group.set_path_constraints(constraints) if fraction < 0.5: group.go() else: group.execute(path) except rospy.ServiceException, e: print "Service call failed: %s"%e
workspace_parameters = WorkspaceParameters() workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = "/BASE" workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = "/BASE" start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"] start_state.joint_state.position = [-0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193] start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jc2 = JointConstraint() jc2.joint_name = "j1" jc2.position = -1.37353344093 jc2.tolerance_above = 0.0001 jc2.tolerance_below = 0.0001 jc2.weight = 1.0 jc3 = JointConstraint() jc3.joint_name = "j2" jc3.position = -1.45013378543 jc3.tolerance_above = 0.0001 jc3.tolerance_below = 0.0001 jc3.weight = 1.0 jc4 = JointConstraint() jc4.joint_name = "j3" jc4.position = 2.18173030842 jc4.tolerance_above = 0.0001
def print_pointlist(self, point_list, future_print_status=False): # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) if future_print_status: self.print_future_visualize(full_point_list, 0, status=self.future_printing_status) self.future_printing_status = 1 # Initial the previous printing point last_ee_pose = point_list[0] # Constraints pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() # joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) # Record how many points has already finished finsih_num = 0 print_num = 0 index_check = 0 while len(point_list) > 0: print('New Plan, points left:', len(point_list), point_list) # Move the robot point to first point and find the height if len(point_list) > 1: initial_plan = self.move_to_initial(point_list[1]) else: initial_plan = self.move_to_initial(point_list[0]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print('plan', fraction) executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=True) executing_state = 1 success_num += 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=True) executing_state = 1 break # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) ## 2nd loop ## always re-plan and check for obstacle while executing waypoints executed_waypoint_index = 0 # initial value of nothing success_point_list = point_list[:success_num] print('when plan success, move_group_status:', self.move_group_execution_status) if executing_state == 1: # if len(full_point_list) != len(point_list): self.future_printing_status = 1 # print 'success planned waypoint\n', success_planned_waypoint_array # print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3 and len( success_point_list) > 0: print(len(success_point_list)) success_planned_waypoint_array = np.delete( np.array(success_point_list), 2, axis=1) if self.waypoint_execution_status == 4 or self.move_group_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.group.stop() executing_state = 0 self.group.clear_pose_targets() break # if self.waypoint_execution_status == 1: # # Check for enclosure # self.base_group.set_position_target([0, 0, 0.05], self.base_group.get_end_effector_link()) # result = self.base_group.plan() # self.base_group.clear_pose_targets() # if len(result.joint_trajectory.points) == 0: # print('Check enclosure failed') # self.group.stop() # executing_state = 0 # break # else: print('Check enclosure successful') current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) # Add current printing obstacle # self.printing_visualize(last_ee_pose, (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z), # name = 'printing point') # Update previous printing point last_ee_pose = (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) del (point_list[:executed_waypoint_index + 1]) del (success_point_list[:executed_waypoint_index + 1]) index_check = self.check_executed_waypoint_index( full_point_array, current_ee_position_array) print 'index check:', index_check, executed_waypoint_index # print 'status:', self.waypoint_execution_status if future_print_status: if index_check >= self.further_printing_number: self.print_future_visualize( full_point_list, index_check, status=self.future_printing_status, point_num=index_check - self.further_printing_number) self.further_printing_number = index_check ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose_smooth = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose_smooth)) # discard the executed waypoints new_point_list = copy.deepcopy( success_point_list[executed_waypoint_index + 1:]) # Add future printing obstacle for k in new_point_list: (current_ee_pose_smooth.position.x, current_ee_pose_smooth.position.y, current_ee_pose_smooth.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose_smooth)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, 0.01, 0.00, path_constraints=constraints) if fraction2 < 0.95: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared along the planned path', 'fraction2:', fraction2 self.group.stop() executing_state = 0 break if self.waypoint_execution_status == 3: # waypoint successfully printed print 'status 3', executed_waypoint_index, point_list[: success_num] # del(point_list[0:executed_waypoint_index+1]) finsih_num += executed_waypoint_index + 1 elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. print 'status 4', executed_waypoint_index, point_list[: executed_waypoint_index + 1] # del(point_list[:executed_waypoint_index+1]) # delete up till whatever is executed finsih_num += executed_waypoint_index + 1 self.group.clear_pose_targets() self.group.stop() executing_state = 0 print('point list left:', len(point_list)) self.msg_print.data = False self.group.set_path_constraints(None) print 'all finish'
def main(argv): # tf_buffer = tf2_ros.Buffer() # tf_listener = tf2_ros.TransformListener(tf_buffer) # target_detect_pose = tf_buffer.lookup_transform("end_effector", "base_footprint", rospy.Time()) listener = tf.TransformListener() listener.waitForTransform('/base_footprint', '/end_effector', rospy.Time(), rospy.Duration(4.0)) (trans, quat) = listener.lookupTransform('/base_footprint', '/end_effector', rospy.Time(0)) client = actionlib.SimpleActionClient( '/plan_and_execute_pose_w_joint_constraints', PlanExecutePoseConstraintsAction) client.wait_for_server() goal = PlanExecutePoseConstraintsGoal() # exit(0) try: goal.target_pose.header.frame_id = argv[0] goal.target_pose.pose.position.x = trans[0] goal.target_pose.pose.position.y = trans[1] goal.target_pose.pose.position.z = trans[2] - 0.13 # theta = math.atan2(goal.target_pose.pose.position.y, goal.target_pose.pose.position.x) # quat = quaternion_from_euler(0.0, 0.0, theta) # print quat goal.target_pose.pose.orientation.x = quat[0] goal.target_pose.pose.orientation.y = quat[1] goal.target_pose.pose.orientation.z = quat[2] goal.target_pose.pose.orientation.w = quat[3] joint_constraint_body_rotate_joint = JointConstraint() joint_constraint_body_rotate_joint.joint_name = "body_rotate_joint" joint_constraint_body_rotate_joint.position = float(0.0) joint_constraint_body_rotate_joint.tolerance_above = float( 5) * math.pi / 180.0 joint_constraint_body_rotate_joint.tolerance_below = float( 5) * math.pi / 180.0 joint_constraint_body_rotate_joint.weight = 1.0 # joint_constraint_arm4_joint = JointConstraint() # joint_constraint_arm4_joint.joint_name = "arm4_joint" # joint_constraint_arm4_joint.position = float(0.0) # joint_constraint_arm4_joint.tolerance_above = float(10) * math.pi / 180.0 # joint_constraint_arm4_joint.tolerance_below = float(10) * math.pi / 180.0 # joint_constraint_arm4_joint.weight = 1.0 # joint_constraint_arm6_joint = JointConstraint() # joint_constraint_arm6_joint.joint_name = "arm6_joint" # joint_constraint_arm6_joint.position = float(0.0) # joint_constraint_arm6_joint.tolerance_above = float(10) * math.pi / 180.0 # joint_constraint_arm6_joint.tolerance_below = float(10) * math.pi / 180.0 # joint_constraint_arm6_joint.weight = 1.0 goal.joint_constraints.append(joint_constraint_body_rotate_joint) # goal.joint_constraints.append(joint_constraint_arm6_joint) except ValueError: quit() client.send_goal(goal) client.wait_for_result() print client.get_result()
def print_pointlist(self, point_list): # Save original points list all_point_list = point_list pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() last_ee_pose = self.group.get_current_pose().pose #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) j = 0 # Record how many points has already finished finsih_num = 0 print_num = 0 while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = self.move_to_initial(point_list[1]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=False) executing_state = 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=False) executing_state = 1 break ## 2nd loop ## always re-plan and check for obstacle while executing waypoints executed_waypoint_index = 0 # initial value of nothing if executing_state == 1: success_planned_waypoint_array = np.delete(np.array( point_list[:success_num]), 2, axis=1) print 'success planned waypoint\n', success_planned_waypoint_array print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3: if self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.group.stop() executing_state = 0 current_ee_pose = self.group.get_current_pose().pose self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \ (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z)) last_ee_pose = current_ee_pose break current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) self.printing_visualize((last_ee_pose.position.x, last_ee_pose.position.y, last_ee_pose.position.z), \ (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z)) last_ee_pose = current_ee_pose executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) # print 'last_ee', (last_ee_pose.position.x,last_ee_pose.position.y), \ # 'current_ee', (current_ee_pose.position.x,current_ee_pose.position.y), \ # 'executed latest index', executed_waypoint_index # print(executed_waypoint_index) # print 'current number', finsih_num + executed_waypoint_index, 'printing number', print_num # if finsih_num + executed_waypoint_index - print_num <= 2 and finsih_num + executed_waypoint_index - print_num >=-2: # self.print_future_visualize(all_point_list, print_num) # print_num = finsih_num + executed_waypoint_index + 1 # # print(success_planned_waypoint_array.size) # print('printing') # print(finsih_num + executed_waypoint_index) # next_future_ob_index = finsih_num + executed_waypoint_index # if next_future_ob_index + 9 < len(all_point_list): # self.future_visualize(all_point_list[next_future_ob_index + 8], all_point_list[next_future_ob_index + 9]) ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose_smooth = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose_smooth)) # discard the executed waypoints new_point_list = point_list[executed_waypoint_index + 1:success_num] # Add future printing obstacle for k in new_point_list: (current_ee_pose_smooth.position.x, current_ee_pose_smooth.position.y, current_ee_pose_smooth.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose_smooth)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, 0.01, 0.00, path_constraints=constraints) if fraction2 < 1.0: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared along the planned path' self.group.stop() executing_state = 0 break j += 1 if j == 2: self.scene.addBox('boxb', 0.5, 0.5, 0.5, 2.5, 0.5, 0.15) rospy.sleep(0.01) if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) # print 'status 3', point_list[:success_num] del (point_list[0:success_num - 1]) finsih_num += success_num elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. # only printed partial waypoint # self.print_list_visualize(point_list[:executed_waypoint_index+1]) # print 'status 4', point_list[:executed_waypoint_index+1] del (point_list[:executed_waypoint_index + 1] ) # delete up till whatever is executed finsih_num += executed_waypoint_index + 1 self.msg_print.data = False self.group.set_path_constraints(None) print 'all finish'
def right_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm right_arm = self.right_arm # 获取当前末端执行器位置姿态 pose_goal = right_arm.get_current_pose().pose # 限制末端夹具运动 right_joint_const = JointConstraint() right_joint_const.joint_name = "gripper_r_joint_r" right_joint_const.position = 0 right_joint_const.weight = 1.0 # 限制1轴转动 right_joint1_const = JointConstraint() right_joint1_const.joint_name = "yumi_joint_1_r" right_joint1_const.position = 0 right_joint1_const.tolerance_above = 120 right_joint1_const.tolerance_below = 0 right_joint1_const.weight = 1.0 # 限制2轴转动 right_joint2_const = JointConstraint() right_joint2_const.joint_name = "yumi_joint_2_r" right_joint2_const.position = 0 right_joint2_const.tolerance_above = 0 right_joint2_const.tolerance_below = 150 right_joint2_const.weight = 1.0 # 限制3轴转动 right_joint3_const = JointConstraint() right_joint3_const.joint_name = "yumi_joint_3_r" right_joint3_const.position = 0 right_joint3_const.tolerance_above = 35 right_joint3_const.tolerance_below = 55 right_joint3_const.weight = 1.0 # 限制4轴转动 right_joint4_const = JointConstraint() right_joint4_const.joint_name = "yumi_joint_4_r" right_joint4_const.position = 0 right_joint4_const.tolerance_above = 60 right_joint4_const.tolerance_below = 75 right_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_r" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 right_joint6_const = JointConstraint() right_joint6_const.joint_name = "yumi_joint_6_r" right_joint6_const.position = 0 right_joint6_const.tolerance_above = 10 right_joint6_const.tolerance_below = 35 right_joint6_const.weight = 1.0 # 限制7轴转动 right_joint7_const = JointConstraint() right_joint7_const.joint_name = "yumi_joint_7_r" right_joint7_const.position = -10 right_joint7_const.tolerance_above = 0 right_joint7_const.tolerance_below = 160 right_joint7_const.weight = 1.0 # 限制末端位移 right_position_const = PositionConstraint() right_position_const.header = Header() right_position_const.link_name = "gripper_r_joint_r" right_position_const.target_point_offset.x = 0.5 right_position_const.target_point_offset.y = -0.5 right_position_const.target_point_offset.z = 1.0 right_position_const.weight = 1.0 # 添加末端姿态约束: right_orientation_const = OrientationConstraint() right_orientation_const.header = Header() right_orientation_const.orientation = pose_goal.orientation right_orientation_const.link_name = "gripper_r_finger_r" right_orientation_const.absolute_x_axis_tolerance = 0.50 right_orientation_const.absolute_y_axis_tolerance = 0.25 right_orientation_const.absolute_z_axis_tolerance = 0.50 right_orientation_const.weight = 100 # 施加全约束 consts = Constraints() consts.joint_constraints = [right_joint_const] # consts.orientation_constraints = [right_orientation_const] # consts.position_constraints = [right_position_const] right_arm.set_path_constraints(consts) # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = Right_Qux # pose_goal.orientation.y = Right_Quy # pose_goal.orientation.z = Right_Quz # pose_goal.orientation.w = Right_Quw # pose_goal.position.x = (Neurondata[5]-0.05)*1.48+0.053 # pose_goal.position.y = (Neurondata[3]+0.18)*1.48-0.12 # pose_goal.position.z = (Neurondata[4]-0.53)*1.48+0.47 # right_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal global n # 设置动作对象目标位置姿态 if n == 1: pose_goal.orientation.x = -0.00825 pose_goal.orientation.y = 0.03556 pose_goal.orientation.z = 0.79932 pose_goal.orientation.w = 0.5998 pose_goal.position.x = 0.54425 pose_goal.position.y = -0.2208 pose_goal.position.z = 0.14822 elif n == 2: pose_goal.orientation.x = -0.00507 pose_goal.orientation.y = -0.012593 pose_goal.orientation.z = 0.98844 pose_goal.orientation.w = 0.15101 pose_goal.position.x = 0.55198 pose_goal.position.y = -0.06859 pose_goal.position.z = 0.17909 elif n == 3: pose_goal.orientation.x = -0.05578 pose_goal.orientation.y = 0.025377 pose_goal.orientation.z = 0.99365 pose_goal.orientation.w = -0.095267 pose_goal.position.x = 0.36478 pose_goal.position.y = -0.05781 pose_goal.position.z = 0.15907 n = 0 right_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # 规划和输出动作 traj = right_arm.plan() right_arm.execute(traj, wait=False) # 动作完成后清除目标信息 right_arm.clear_pose_targets() # 清除路径约束 right_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 right_arm.stop()
result = fk.getFK('arm_right_7_link', current_joint_states.name, correct_js.position, 'base_link') rospy.loginfo("Result of current robot pose FK is: " + str(result)) rs = RobotState() rs.joint_state = correct_js drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published current robot state") rospy.sleep(2.0) c = Constraints() jc = JointConstraint() jc.joint_name = 'arm_right_1_link' jc.position = 0.0 jc.tolerance_above = 0.00001 jc.tolerance_below = 0.00001 jc.weight = 1.0 c.joint_constraints.append(jc) rospy.loginfo("Result without constraints:") resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], False, 0, rs) rospy.loginfo(str(resultik)) rs = RobotState() rs.joint_state = resultik.solution.joint_state drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs) rospy.loginfo("Published solution with False collision avoidance robot state")
def update(self): self.logger.debug("{0}.update()".format(self.__class__.__name__)) if not self.action_client: self.feedback_message = "no action client, did you call setup() on your tree?" return py_trees.Status.INVALID # pity there is no 'is_connected' api like there is for c++ if not self.sent_goal: # if self.blackboard.frame_id != self.blackboard.target: # print("Detected fail!") # return py_trees.Status.SUCCESS #update goal data from blackboard self.action_goal.target_pose.header.frame_id = "base_footprint" self.action_goal.target_pose.pose.position.x = self.blackboard.qrcode_pose.pose.position.x # + self.x_offset self.action_goal.target_pose.pose.position.y = self.blackboard.qrcode_pose.pose.position.y # + self.y_offset self.action_goal.target_pose.pose.position.z = self.blackboard.qrcode_pose.pose.position.z # + self.z_offset #self.action_goal.target_pose.pose.position.x = self.x_offset #self.action_goal.target_pose.pose.position.y = self.y_offset #self.action_goal.target_pose.pose.position.z = self.z_offset # rospy.loginfo(self.action_goal.target_pose.pose.position.x) theta = math.atan2(self.action_goal.target_pose.pose.position.y, self.action_goal.target_pose.pose.position.x) quat = quaternion_from_euler(0.0, 0.0, theta) self.action_goal.target_pose.pose.orientation.x = quat[0] self.action_goal.target_pose.pose.orientation.y = quat[1] self.action_goal.target_pose.pose.orientation.z = quat[2] self.action_goal.target_pose.pose.orientation.w = quat[3] # self.action_goal.target_pose.pose.orientation.x = 0 # self.action_goal.target_pose.pose.orientation.y = 0 # self.action_goal.target_pose.pose.orientation.z = 0 # self.action_goal.target_pose.pose.orientation.w = 1.0 if self.constraint: if self.joint == "": print("Constraint joint not defined.") return py_trees.Status.FAILURE joint_constraint = JointConstraint() joint_constraint.joint_name = self.joint joint_constraint.position = 0.0 joint_constraint.tolerance_above = 30 * math.pi / 180.0 joint_constraint.tolerance_below = 30 * math.pi / 180.0 joint_constraint.weight = 1.0 # arm6_joint makes singularity, so always constraint the arm6_joint. joint_constraint_arm6_joint = JointConstraint() joint_constraint_arm6_joint.joint_name = "arm6_joint" joint_constraint_arm6_joint.position = float(0.0) joint_constraint_arm6_joint.tolerance_above = float( 10) * math.pi / 180.0 joint_constraint_arm6_joint.tolerance_below = float( 10) * math.pi / 180.0 joint_constraint_arm6_joint.weight = 1.0 self.action_goal.joint_constraints.append(joint_constraint) self.action_goal.joint_constraints.append( joint_constraint_arm6_joint) print("Joint constrained.") # print("<== action goal ==>") # print("Find : " + self.blackboard.frame_id) # print("Goal position : (" + str(self.action_goal.target_pose.pose.position.x) # + " , " + str(self.action_goal.target_pose.pose.position.y) # + " , " + str(self.action_goal.target_pose.pose.position.z) + ") away from /base_footprint") print("Goal position : (" + str(self.action_goal.target_pose.pose.position.x) + " , " + str(self.action_goal.target_pose.pose.position.y) + " , " + str(self.action_goal.target_pose.pose.position.z) + ")") print("Goal orientation : ", quat) self.action_client.send_goal(self.action_goal) self.sent_goal = True self.feedback_message = "sent goal to the action server" return py_trees.Status.RUNNING self.feedback_message = self.action_client.get_goal_status_text() if self.action_client.get_state() in [ actionlib_msgs.GoalStatus.ABORTED, actionlib_msgs.GoalStatus.PREEMPTED ]: return py_trees.Status.FAILURE result = self.action_client.get_result() if result: return py_trees.Status.SUCCESS else: self.feedback_message = self.override_feedback_message_on_running return py_trees.Status.RUNNING
workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = "/BASE" start_state.joint_state.name = ["j1", "j2", "j3", "j4", "j5", "flange"] start_state.joint_state.position = [ -0.2569046038066598, -0.8442722962923348, 1.849082034218144, 0.26825374068443164, -0.04090683809444329, 5.745512865657193 ] start_state.joint_state.velocity = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] jc2 = JointConstraint() jc2.joint_name = "j1" jc2.position = -1.37353344093 jc2.tolerance_above = 0.0001 jc2.tolerance_below = 0.0001 jc2.weight = 1.0 jc3 = JointConstraint() jc3.joint_name = "j2" jc3.position = -1.45013378543 jc3.tolerance_above = 0.0001 jc3.tolerance_below = 0.0001 jc3.weight = 1.0 jc4 = JointConstraint() jc4.joint_name = "j3" jc4.position = 2.18173030842 jc4.tolerance_above = 0.0001
def joint_position_callback(joints): global plan_only fixed_frame = rospy.get_param("/fixed_frame") client = actionlib.SimpleActionClient('move_group', MoveGroupAction) client.wait_for_server() move_group_goal = MoveGroupGoal() try: msg = MotionPlanRequest() workspace_parameters = WorkspaceParameters() workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = fixed_frame workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = fixed_frame start_state.joint_state.name = [] start_state.joint_state.position = [] cons = Constraints() cons.name = "" i = 0 for dim in joints.start_joint.layout.dim: start_state.joint_state.name.append(dim.label) start_state.joint_state.position.append(joints.start_joint.data[i]) jc = JointConstraint() jc.joint_name = dim.label jc.position = joints.goal_joint.data[i] jc.tolerance_above = 0.0001 jc.tolerance_below = 0.0001 jc.weight = 1.0 i = i + 1 cons.joint_constraints.append(jc) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = joints.group_name move_group_goal.request = msg if joints.plan_only: plan_only = True move_group_goal.planning_options.plan_only = True else: plan_only = False client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e: print "failed: %s"%e
def print_pointlist(self, point_list): # Get differential of way point list differential = self.get_way_point_differential(point_list) differential_num = [0] differential_num = differential_num + differential eef_rotation_change = [0, 0] for i in range(2, len(differential_num)): eef_rotation_change.append(differential_num[i] - differential_num[i - 1]) pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains # constraints = Constraints() orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi constraints.orientation_constraints.append(orientation_constraint) while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = self.move_to_initial(point_list[1]) joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot.get_current_state().joint_state.position) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] (roll, pitch, yaw) = euler_from_quaternion([ wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w ]) # [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ # quaternion_from_euler(roll, pitch, yaw + eef_rotation_change[point_num]) # print(yaw + eef_rotation_change[point_num]) [wpose.orientation.x, wpose.orientation.y, wpose.orientation.z, wpose.orientation.w] = \ quaternion_from_euler(roll, pitch, yaw) waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan) self.print_list_visualize(point_list) elif success_num == 0: break else: # execute success plan self.msg_prit.data = True self.group.execute(execute_plan) self.print_list_visualize(point_list[:success_num]) break self.msg_prit.data = False # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) del (eef_rotation_change[0:success_num - 1]) # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) print 'all finish'
def print_pointlist(self, point_list): ##Add constrain pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = self.move_to_initial(point_list[1]) joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot.get_current_state().joint_state.position) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan) elif success_num == 0: break else: # execute success plan self.msg_prit.data = True self.group.execute(execute_plan) break self.msg_prit.data = False # Delete the points what already execute if success_num > 0: del (point_list[0:success_num - 1]) # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) print 'all finish'
# Notes on getting jacobian etc. # Install pykdl_utils by checking out git into catkin_ws and catkin_make_isolated if __name__ == '__main__': rospy.init_node('touch_table', anonymous=True) moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "panda_arm" move_group = moveit_commander.MoveGroupCommander(group_name) path_constr = Constraints() path_constr.name = "arm_constr" joint_constraint = JointConstraint() joint_constraint.position = 0.8 joint_constraint.tolerance_above = 3.14 joint_constraint.tolerance_below = 0.1 joint_constraint.weight = 1 joint_constraint.joint_name = "panda_joint2" path_constr.joint_constraints.append(joint_constraint) move_group.set_path_constraints(path_constr) try: touch_and_refine_table(robot, scene, move_group) except KeyboardInterrupt: print("Shutting down")
def print_pointlist(self, point_list, future_print_status=False, name=None): msg_experiment = String() msg_experiment = name self.experiment.publish(msg_experiment) startime = datetime.datetime.now() # Record number of print part number_printing_part = 0 # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) self.target_list = full_point_list if future_print_status: self.future_printing_status = True # Constraints pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() # joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) # Record how many points has already finished finsih_num = 0 print_num = 0 index_check = 0 while len(point_list) > 0: print('New Plan, points left:', len(point_list)) # Move the robot point to first point and find the height if len(point_list) > 1: initial_plan = self.move_to_initial(point_list[1]) else: initial_plan = self.move_to_initial(point_list[0]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Way points plan_start_time = datetime.datetime.now() waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Adding the first planing point, and fraction is', fraction executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=False) self.msg_print.data = True executing_state = 1 success_num += 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=False) executing_state = 1 break plan_end_time = datetime.datetime.now() print 'first planing point time is', (plan_end_time - plan_start_time).seconds self.planning_time += max( (plan_end_time - plan_start_time).seconds, 1) ## 2nd loop ## always re-plan and check for obstacle while executing waypoints repeat_check = 0 previous_index = 0 if future_print_status == True: # Check for enclosure self.base_group.set_position_target( [0, 0, 0.05], self.base_group.get_end_effector_link()) result = self.base_group.plan() self.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') self.group.stop() print('Removing future obstacle') self.future_printing_status = False self.enclosure(point_list[0]) break else: print('Check enclosure successful') executed_waypoint_index = 0 # initial value of nothing success_point_list = point_list[:success_num] print('when plan success, move_group_status:', self.move_group_execution_status, 'success_plan_number:', success_num) if executing_state == 1: printing_start_time = datetime.datetime.now() success_planned_waypoint_array = np.delete(np.array( point_list[:success_num]), 2, axis=1) # print 'success planned waypoint\n', success_planned_waypoint_array print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3: if point_list == []: break if self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.msg_print.data = False self.group.stop() executing_state = 0 break if self.waypoint_execution_status == 3: break current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) executed_waypoint_index = max( self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array), executed_waypoint_index) if executed_waypoint_index == previous_index: repeat_check += 1 previous_index = executed_waypoint_index if repeat_check >= 20: break # print 'executed latest index', executed_waypoint_index index_check = max( self.check_executed_waypoint_index( full_point_array, current_ee_position_array), index_check) self.further_printing_number = index_check print 'index:', index_check, 'way_point index', executed_waypoint_index if future_print_status == True: # Check for enclosure self.base_group.set_position_target( [0, 0, 0.05], self.base_group.get_end_effector_link()) result = self.base_group.plan() self.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') self.group.stop() print('Removing future obstacle') self.future_printing_status = False self.enclosure(full_point_list[index_check]) break else: print('Check enclosure successful') ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose)) # discard the executed waypoints new_point_list = point_list[ executed_waypoint_index:success_num] for k in new_point_list: (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Dynamic check fraction:', fraction2 if fraction2 < 0.95: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared to be in the path' self.group.stop() self.msg_print.data = False executing_state = 0 break rospy.sleep(2) printing_end_time = datetime.datetime.now() self.printing_time += (printing_end_time - printing_start_time).seconds number_printing_part += 1 print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) del (point_list[:success_num - 1]) elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. # only printed partial waypoint # self.print_list_visualize(point_list[:executed_waypoint_index+1]) if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully. del (point_list[:executed_waypoint_index] ) # delete up till whatever is executed self.msg_print.data = False if point_list == []: rospy.sleep(2) self.group.stop() # Delete the points what already execute # if success_num > 0: # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) finshtime = datetime.datetime.now() print('All time:', (finshtime - startime).seconds) print('Printing time:', self.printing_time) print('planning time:', self.planning_time) print('Travel time:', (finshtime - startime).seconds - self.printing_time - self.planning_time) print('number of printing:', number_printing_part) # Tell plot node save data msg_experiment = String() msg_experiment = 'Finish' self.experiment.publish(msg_experiment) # Save result to csv list_result = [[(finshtime - startime).seconds, self.printing_time, self.planning_time, (finshtime - startime).seconds - self.printing_time - self.planning_time, number_printing_part]] result_name = [ 'All time', 'Printing time', 'planning time', 'Travel time', 'number of printing' ] result_data = pd.DataFrame(columns=result_name, data=list_result) # Plot Result full_point_list_x = [base[0] for base in full_point_list] full_point_list_y = [base[1] for base in full_point_list] plt3 = plt.figure("Printing result", figsize=(6, 6)) plt.plot(self.re_position_x, self.re_position_y, 'b', linewidth=1.2) plt.plot(full_point_list_x, full_point_list_y, 'ro') plt.xlim(min(full_point_list_x) - 0.1, max(full_point_list_x) + 0.1) plt.ylim(min(full_point_list_y) - 0.1, max(full_point_list_y) + 0.1) plt.legend(['Printing result', 'ground truth'], fontsize=8, bbox_to_anchor=(1.0, 1)) plt.xlabel("X axis (m)") plt.ylabel("y axis (m)") plt.title('Printing result') plt.show() # Save printing path printing_result_list = [ full_point_list_x, full_point_list_y, self.re_position_x, self.re_position_y ] printing_data = pd.DataFrame(data=printing_result_list) # Save result plot if name: plt3.savefig('experiment_data/' + str(name) + '/' + str(name) + '_result.png', dpi=plt3.dpi) result_data.to_csv('experiment_data/' + str(name) + '/' + str(name) + '_result.csv', encoding='gbk') printing_data.to_csv('experiment_data/' + str(name) + '/' + str(name) + '_printpath.csv', encoding='gbk') print('All finish')
def main(): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = [ 'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print 'Enter start state:' try: for i in range(len(joint_names)): val = float(raw_input(joint_names[i] + ':')) goal.request.start_state.joint_state.position[i] = val except: print 'Using defaults for remaining joints' print goal.request.start_state.joint_state.position #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] target_joint_angles = [ 0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] print 'Enter goal state:' try: for i in range(len(target_joint_angles)): val = float(raw_input(joint_names[i] + ':')) target_joint_angles[i] = val except: print 'Using defaults for remaining joints' print target_joint_angles tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append( Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def joint_position_callback(joints): global plan_only fixed_frame = rospy.get_param("/fixed_frame") client = actionlib.SimpleActionClient('move_group', MoveGroupAction) client.wait_for_server() move_group_goal = MoveGroupGoal() try: msg = MotionPlanRequest() workspace_parameters = WorkspaceParameters() workspace_parameters.header.stamp = rospy.Time.now() workspace_parameters.header.frame_id = fixed_frame workspace_parameters.min_corner = Vector3(-1.0, -1.0, -1.0) workspace_parameters.max_corner = Vector3(1.0, 1.0, 1.0) start_state = RobotState() # start_state.joint_state.header.stamp = rospy.Time.now() start_state.joint_state.header.frame_id = fixed_frame start_state.joint_state.name = [] start_state.joint_state.position = [] cons = Constraints() cons.name = "" i = 0 for dim in joints.start_joint.layout.dim: start_state.joint_state.name.append(dim.label) start_state.joint_state.position.append(joints.start_joint.data[i]) jc = JointConstraint() jc.joint_name = dim.label jc.position = joints.goal_joint.data[i] jc.tolerance_above = 0.0001 jc.tolerance_below = 0.0001 jc.weight = 1.0 i = i + 1 cons.joint_constraints.append(jc) msg.workspace_parameters = workspace_parameters msg.start_state = start_state msg.goal_constraints.append(cons) msg.num_planning_attempts = 1 msg.allowed_planning_time = 5.0 msg.group_name = joints.group_name move_group_goal.request = msg if joints.plan_only: plan_only = True move_group_goal.planning_options.plan_only = True else: plan_only = False client.send_goal(move_group_goal) client.wait_for_result(rospy.Duration.from_sec(5.0)) except rospy.ROSInterruptException, e: print "failed: %s" % e
def plan_trajectory(self, start_point, goal_point, planner_number, joint_names, group_name, planning_time, planner_config_name): """ Given a start and goal point, returns the planned path. Args: start_point (list of float): A starting joint configuration. goal_point (list of float): A goal joint configuration. planner_number (int): The index of the planner to be used as returned by acquire_planner(). joint_names (list of str): The name of the joints corresponding to start_point and goal_point. group_name (str): The name of the group to which the joint names correspond. planning_time (float): Maximum allowed time for planning, in seconds. planner_config_name (str): Type of planner to use. Return: list of list of float: A sequence of points representing the joint configurations at each point on the path. """ planner_client = rospy.ServiceProxy(self.planners[planner_number], GetMotionPlan) rospy.loginfo( "Plan Trajectory Wrapper: got a plan_trajectory request for %s with start = %s and goal = %s" % (self.planners[planner_number], start_point, goal_point)) # Put together the service request. req = GetMotionPlanRequest() req.motion_plan_request.workspace_parameters.header.stamp = rospy.get_rostime( ) req.motion_plan_request.group_name = group_name req.motion_plan_request.num_planning_attempts = 1 req.motion_plan_request.allowed_planning_time = planning_time req.motion_plan_request.planner_id = planner_config_name #using RRT planner by default req.motion_plan_request.start_state.joint_state.header.stamp = rospy.get_rostime( ) req.motion_plan_request.start_state.joint_state.name = joint_names req.motion_plan_request.start_state.joint_state.position = start_point req.motion_plan_request.goal_constraints.append(Constraints()) req.motion_plan_request.goal_constraints[0].joint_constraints = [] for i in xrange(len(joint_names)): temp_constraint = JointConstraint() temp_constraint.joint_name = joint_names[i] temp_constraint.position = goal_point[i] temp_constraint.tolerance_above = 0.05 temp_constraint.tolerance_below = 0.05 req.motion_plan_request.goal_constraints[ 0].joint_constraints.append(temp_constraint) #call the planner rospy.wait_for_service(self.planners[planner_number]) rospy.loginfo("Plan Trajectory Wrapper: sent request to service %s" % planner_client.resolved_name) try: response = planner_client(req) except rospy.ServiceException, e: rospy.loginfo("Plan Trajectory Wrapper: service call failed: %s" % e) return None
def cw3_example_script(): """ This script will go through the main aspects of moveit and the components you will need to complete the coursework. You can find more information on """ # Initialize moveit_commander and rospy node moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('move_group_python_interface') # Initialize moveit scene interface (woeld surrounding the robot) scene = moveit_commander.PlanningSceneInterface() # Robot contains the entire state of the robot (iiw a and shadow hand) global group, base_group, arm_group # request_prin = rospy.ServiceProxy('set_extruder_printing', SetBool) msg_prit = SetBoolRequest() request_fk = rospy.ServiceProxy('/compute_fk', GetPositionFK) robot = moveit_commander.RobotCommander() group = moveit_commander.MoveGroupCommander('robot') arm_group = moveit_commander.MoveGroupCommander('arm') # gripeer_group = moveit_commander.MoveGroupCommander('gripper') base_group = moveit_commander.MoveGroupCommander('base') # Create a DisplayTrajectory publisher which is used later to publish trajectories for RViz to visualize: display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) extruder_publisher = rospy.Publisher('set_extruder_rate', Float32, queue_size=20) msg_extrude = Float32() msg_extrude = 5.0 extruder_publisher.publish(msg_extrude) group.allow_replanning(True) group.allow_looking(True) # Set the number of planning attempts to find better solution group.set_num_planning_attempts(10) ################################################ ###Add obstacle rospy.sleep(0.2) box_pose = geometry_msgs.msg.PoseStamped() box_pose.header.frame_id = "odom" box_pose.pose.position.x = 0 box_pose.pose.position.y = 2 box_pose.pose.position.z = 0.01 box_name = "wall" scene.add_box(box_name, box_pose, size=(2, 2, 0.01)) rospy.sleep(0.2) ### Add cylinder # p = PlanningSceneInterface("base_link") # p.addCylinder("Cylinder", 0.01, 0.5, -1, 1, 0, wait=False) # p.addMesh("iron_man", 0.01, 0.5, , wait=False) p = PoseStamped() p.header.frame_id = "odom" p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.05 p.pose.orientation.x = 0 p.pose.orientation.y = 0 p.pose.orientation.z = 0 p.pose.orientation.w = 1 scene.add_mesh("cylinder",p,'model/cylinder.stl') rospy.sleep(0.2) return ############################################################# point_list = [] for i in range(10): point_list.append((i * 0.2 -1, 1, 0.1)) for i in range(10): point_list.append((1, i * 0.2 + 1, 0.1)) for i in range(10): point_list.append((i * (-0.2) + 1, 3, 0.1)) for i in range(10): point_list.append((-1, i * -0.2 + 3, 0.1)) point_list.append(point_list[0]) print(point_list) ############################################################# ######################################################################## ##Add constrain pose = group.get_current_pose(group.get_end_effector_link()) constraints = Constraints() # orientation_constraint = OrientationConstraint() # orientation_constraint.header = pose.header # orientation_constraint.link_name = group.get_end_effector_link() # orientation_constraint.orientation = pose.pose.orientation # orientation_constraint.absolute_x_axis_tolerance = 0.1 # orientation_constraint.absolute_y_axis_tolerance = 0.1 # orientation_constraint.absolute_z_axis_tolerance = 2*pi # current_orientation_list = [pose.pose.orientation.x, # pose.pose.orientation.y, # pose.pose.orientation.z, # pose.pose.orientation.w] # # get euler angle from quaternion # (roll, pitch, yaw) = euler_from_quaternion(current_orientation_list) # pitch = pi # roll = 0 # orientation_constraint.weight = 1 # [orientation_constraint.orientation.x, orientation_constraint.orientation.y, orientation_constraint.orientation.z, orientation_constraint.orientation.w] = \ # quaternion_from_euler(roll, pitch, yaw) # constraints.orientation_constraints.append(orientation_constraint) # group.set_path_constraints(constraints) #### joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169*pi/180 joint_constraint.tolerance_above = 30*pi/180 joint_constraint.tolerance_below = 30*pi/180 joint_constraint.weight = 1 constraints.joint_constraints.append(joint_constraint) group.set_path_constraints(constraints) ####################################################################### while len(point_list) > 1: # Move the robot point to first point and find the height initial_plan = move_to_initial(point_list[1]) joint_goal = group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = robot.get_current_state() # print(robot.get_current_state().joint_state.position) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) resp = request_fk(head, [group.get_end_effector_link()], robot_state) current_pose = group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] group.set_pose_target(current_pose) group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): group.execute(execute_plan) else: # execute success plan msg_prit.data = True group.execute(execute_plan) break msg_prit.data = False # Delete the points what already execute if success_num > 0: del(point_list[0:success_num-1]) ############################################################# print 'all finish' # When finished shut down moveit_commander. moveit_commander.roscpp_shutdown()
def update(self): self.logger.debug("{0}.update()".format(self.__class__.__name__)) if not self.action_client: self.feedback_message = "no action client, did you call setup() on your tree?" return py_trees.Status.INVALID # pity there is no 'is_connected' api like there is for c++ if not self.sent_goal: self.listener.waitForTransform('/base_footprint', '/end_effector', rospy.Time(), rospy.Duration(4.0)) (trans, quat) = self.listener.lookupTransform('/base_footprint', '/end_effector', rospy.Time(0)) #update goal data from blackboard self.action_goal.target_pose.header.frame_id = "base_footprint" self.action_goal.target_pose.pose.position.x = trans[ 0] + self.x_offset self.action_goal.target_pose.pose.position.y = trans[ 1] + self.y_offset self.action_goal.target_pose.pose.position.z = trans[ 2] + self.z_offset self.action_goal.target_pose.pose.orientation.x = quat[0] self.action_goal.target_pose.pose.orientation.y = quat[1] self.action_goal.target_pose.pose.orientation.z = quat[2] self.action_goal.target_pose.pose.orientation.w = quat[3] if self.constraint: if len(self.joint) == 0: print("Constraint joint not defined.") return py_trees.Status.FAILURE for joint in self.joint: joint_constraint = JointConstraint() joint_constraint.joint_name = joint joint_constraint.position = self.joint[ joint_constraint.joint_name][0] joint_constraint.tolerance_above = self.joint[ joint_constraint.joint_name][1] joint_constraint.tolerance_below = self.joint[ joint_constraint.joint_name][2] joint_constraint.weight = 1.0 self.action_goal.joint_constraints.append(joint_constraint) print("Joint constrained : " + str(joint_constraint)) print("Goal position : (" + str(self.action_goal.target_pose.pose.position.x) + " , " + str(self.action_goal.target_pose.pose.position.y) + " , " + str(self.action_goal.target_pose.pose.position.z) + ")") print("Goal orientation : ", quat) self.action_client.send_goal(self.action_goal) self.sent_goal = True self.feedback_message = "sent goal to the action server" return py_trees.Status.RUNNING self.feedback_message = self.action_client.get_goal_status_text() # Failure case if self.action_client.get_state() in [ actionlib_msgs.GoalStatus.ABORTED, actionlib_msgs.GoalStatus.PREEMPTED ]: return py_trees.Status.FAILURE result = self.action_client.get_result() if result: return py_trees.Status.SUCCESS else: self.feedback_message = self.override_feedback_message_on_running return py_trees.Status.RUNNING
def main(p, a): #Initialize the node rospy.init_node('moveit_client') # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient('move_group', MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() #----------------Construct the goal message (start)---------------- joint_names = [ 'head_pan', 'left_s0', 'left_s1', 'left_e0', 'left_e1', 'left_w0', 'left_w1', 'left_w2', 'right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2' ] #Set parameters for the planner goal.request.group_name = 'both_arms' goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 #Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" #Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names if (len(p) == 15): goal.request.start_state.joint_state.position = [ p[0], p[1], p[2], p[3], p[4], p[5], p[6], p[7], p[8], p[9], p[10], p[11], p[12], p[13], p[14] ] else: print('Not enough positions: Need 15') goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] #Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True #Set the goal position of the robot #Note that the goal is specified with a collection of individual #joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] if (len(a) == 15): target_joint_angles = [ a[0], a[1], a[2], a[3], a[4], a[5], a[6], a[7], a[8], a[9], a[10], a[11], a[12], a[13], a[14] ] else: print('Not enough angles: Need 15') tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append( Constraints(name='', joint_constraints=consts)) #---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def left_arm_go_to_pose_goal(self): # 设置动作对象变量,此处为arm left_arm = self.left_arm # 获取当前末端执行器位置姿态 pose_goal = left_arm.get_current_pose().pose # 限制末端夹具运动 left_joint_const = JointConstraint() left_joint_const.joint_name = "gripper_l_joint_r" if Leftfinger > -32: left_joint_const.position = 0.024 else: left_joint_const.position = 0 left_joint_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = -0.5 left_position_const.target_point_offset.z = 1.0 left_position_const.weight = 1.0 # # 限制1轴转动 left_joint1_const = JointConstraint() left_joint1_const.joint_name = "yumi_joint_1_l" left_joint1_const.position = 0 left_joint1_const.tolerance_above = 1.76 left_joint1_const.tolerance_below = 0 left_position_const.weight = 1.0 # 限制2轴转动 left_joint2_const = JointConstraint() left_joint2_const.joint_name = "yumi_joint_2_l" left_joint2_const.position = 0 left_joint2_const.tolerance_above = 0 left_joint2_const.tolerance_below = 150 left_joint2_const.weight = 1.0 # 限制3轴转动 left_joint3_const = JointConstraint() left_joint3_const.joint_name = "yumi_joint_3_l" left_joint3_const.position = 0 left_joint3_const.tolerance_above = 35 left_joint3_const.tolerance_below = 55 left_joint3_const.weight = 1.0 # 限制4轴转动 left_joint4_const = JointConstraint() left_joint4_const.joint_name = "yumi_joint_4_l" left_joint4_const.position = 0 left_joint4_const.tolerance_above = 60 left_joint4_const.tolerance_below = 75 left_joint4_const.weight = 1.0 # 限制5轴转动 right_joint5_const = JointConstraint() right_joint5_const.joint_name = "yumi_joint_5_l" right_joint5_const.position = 40 right_joint5_const.tolerance_above = 50 right_joint5_const.tolerance_below = 20 right_joint5_const.weight = 1.0 # 限制6轴转动 left_joint6_const = JointConstraint() left_joint6_const.joint_name = "yumi_joint_6_l" left_joint6_const.position = 0 left_joint6_const.tolerance_above = 10 left_joint6_const.tolerance_below = 35 left_joint6_const.weight = 1.0 # 限制7轴转动 left_joint7_const = JointConstraint() left_joint7_const.joint_name = "yumi_joint_7_l" left_joint7_const.position = -10 left_joint7_const.tolerance_above = 0 left_joint7_const.tolerance_below = 160 left_joint7_const.weight = 1.0 # 限制末端位移 left_position_const = PositionConstraint() left_position_const.header = Header() left_position_const.link_name = "gripper_l_joint_r" left_position_const.target_point_offset.x = 0.5 left_position_const.target_point_offset.y = 0.25 left_position_const.target_point_offset.z = 0.5 left_position_const.weight = 1.0 # 添加末端姿态约束: left_orientation_const = OrientationConstraint() left_orientation_const.header = Header() left_orientation_const.orientation = pose_goal.orientation left_orientation_const.link_name = "gripper_l_finger_r" left_orientation_const.absolute_x_axis_tolerance = 0.5 left_orientation_const.absolute_y_axis_tolerance = 0.25 left_orientation_const.absolute_z_axis_tolerance = 0.5 left_orientation_const.weight = 1 # 施加全约束 consts = Constraints() consts.joint_constraints = [left_joint_const] # consts.orientation_constraints = [left_orientation_const] # consts.position_constraints = [left_position_const] left_arm.set_path_constraints(consts) # 设置动作对象目标位置姿态 pose_goal.orientation.x = Left_Qux pose_goal.orientation.y = Left_Quy pose_goal.orientation.z = Left_Quz pose_goal.orientation.w = Left_Quw pose_goal.position.x = (Neurondata[11] - 0.05) * 1.48 + 0.053 pose_goal.position.y = (Neurondata[9] - 0.18) * 1.48 + 0.12 pose_goal.position.z = (Neurondata[10] - 0.53) * 1.48 + 0.47 left_arm.set_pose_target(pose_goal) print "End effector pose %s" % pose_goal # # 设置动作对象目标位置姿态 # pose_goal.orientation.x = pose_goal.orientation.x # pose_goal.orientation.y = pose_goal.orientation.y # pose_goal.orientation.z = pose_goal.orientation.z # pose_goal.orientation.w = pose_goal.orientation.w # pose_goal.position.x = pose_goal.position.x # pose_goal.position.y = pose_goal.position.y - 0.01 # pose_goal.position.z = pose_goal.position.z # left_arm.set_pose_target(pose_goal) # print "End effector pose %s" % pose_goal # 规划和输出动作 traj = left_arm.plan() left_arm.execute(traj, wait=False) # 动作完成后清除目标信息 left_arm.clear_pose_targets() # 清除路径约束 left_arm.clear_path_constraints() # 确保没有剩余未完成动作在执行 left_arm.stop()
def print_pointlist(self, point_list, future_print_status=True): # Save original points list full_point_list = copy.deepcopy(point_list) full_point_array = np.delete(np.array(full_point_list), 2, axis=1) self.target_list = full_point_list if future_print_status: self.future_printing_status = True # Constraints pose = self.group.get_current_pose(self.group.get_end_effector_link()) constraints = Constraints() # joint constraints joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_1' joint_constraint.position = 169 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm_joint_4' joint_constraint.position = 150 * pi / 180 joint_constraint.tolerance_above = 30 * pi / 180 joint_constraint.tolerance_below = 30 * pi / 180 joint_constraint.weight = 1.0 constraints.joint_constraints.append(joint_constraint) self.group.set_path_constraints(constraints) # Orientation constrains orientation_constraint = OrientationConstraint() orientation_constraint.header = pose.header orientation_constraint.link_name = self.group.get_end_effector_link() orientation_constraint.orientation = pose.pose.orientation orientation_constraint.absolute_x_axis_tolerance = 2 * pi orientation_constraint.absolute_y_axis_tolerance = 2 * pi orientation_constraint.absolute_z_axis_tolerance = 2 * pi orientation_constraint.weight = 1.0 constraints.orientation_constraints.append(orientation_constraint) # Record how many points has already finished finsih_num = 0 print_num = 0 index_check = 0 while len(point_list) > 0: print('New Plan, points left:', len(point_list), point_list) # Move the robot point to first point and find the height if len(point_list) > 1: initial_plan = self.move_to_initial(point_list[1]) else: initial_plan = self.move_to_initial(point_list[0]) # joint_goal = self.group.get_current_joint_values() head = initial_plan.joint_trajectory.header robot_state = self.robot.get_current_state() # print(robot_state.joint_state) robot_state.joint_state.position = tuple(initial_plan.joint_trajectory.points[-1].positions) + \ tuple(robot_state.joint_state.position[7:]) # the joints for the wheel resp = self.request_fk(head, [self.group.get_end_effector_link()], robot_state) current_pose = self.group.get_current_pose().pose current_pose.orientation = resp.pose_stamped[0].pose.orientation (current_pose.position.x, current_pose.position.y, current_pose.position.z) = point_list[0] self.group.set_pose_target(current_pose) self.group.go() # Move the robot to the center of the striaght line to make sure Way point method can be executed # Way points waypoints = [] wpose = self.group.get_current_pose().pose # Add the current pose to make sure the path is smooth waypoints.append(copy.deepcopy(wpose)) success_num = 0 for point_num in range(len(point_list)): (wpose.position.x, wpose.position.y, wpose.position.z) = point_list[point_num] waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Adding the first planing point, and fraction is', fraction executing_state = 0 if fraction == 1: success_num += 1 execute_plan = plan if success_num == len(point_list): self.group.execute(execute_plan, wait=False) self.msg_extrude = 3.0 self.extruder_publisher.publish(self.msg_extrude) executing_state = 1 success_num += 1 elif success_num == 0: break else: # execute success plan self.msg_print.data = True self.group.execute(execute_plan, wait=False) self.msg_extrude = 3.0 self.extruder_publisher.publish(self.msg_extrude) executing_state = 1 break ## 2nd loop ## always re-plan and check for obstacle while executing waypoints executed_waypoint_index = 0 # initial value of nothing success_point_list = point_list[:success_num] print('when plan success, move_group_status:', self.move_group_execution_status, 'success_plan_number:', success_num) if executing_state == 1: success_planned_waypoint_array = np.delete(np.array( point_list[:success_num]), 2, axis=1) # print 'success planned waypoint\n', success_planned_waypoint_array print 'status', self.waypoint_execution_status while self.waypoint_execution_status != 3: if self.waypoint_execution_status == 4: # aborted state print 'stop and abort waypoint execution' self.msg_print.data = False self.group.stop() executing_state = 0 break current_ee_pose = self.group.get_current_pose().pose current_ee_position_array = np.array([ current_ee_pose.position.x, current_ee_pose.position.y ]) executed_waypoint_index = self.check_executed_waypoint_index( success_planned_waypoint_array, current_ee_position_array) # print 'executed latest index', executed_waypoint_index index_check = self.check_executed_waypoint_index( full_point_array, current_ee_position_array) self.further_printing_number = index_check print 'index:', index_check, 'way_point index', executed_waypoint_index if future_print_status == True: # Check for enclosure self.base_group.set_position_target( [0, 0, 0.05], self.base_group.get_end_effector_link()) result = self.base_group.plan() self.base_group.clear_pose_targets() if len(result.joint_trajectory.points) == 0: print('Check enclosure failed') else: print('Check enclosure successful') ## Replan to check for dynamic obstacle waypoints = [] # Add the current pose to make sure the path is smooth, get latest pose current_ee_pose = self.group.get_current_pose().pose waypoints.append(copy.deepcopy(current_ee_pose)) # discard the executed waypoints new_point_list = point_list[ executed_waypoint_index:success_num] for k in new_point_list: (current_ee_pose.position.x, current_ee_pose.position.y, current_ee_pose.position.z) = k waypoints.append(copy.deepcopy(current_ee_pose)) (plan2, fraction2) = self.group.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.00, path_constraints=constraints) print 'Dynamic check fraction:', fraction2 if fraction2 < 1.0: ## new obstacle appear # print 'executed latest index', executed_waypoint_index # print 'fraction value', fraction,'\n' print 'new obstacle appeared to be in the path' self.group.stop() self.msg_print.data = False executing_state = 0 break rospy.sleep(2) print 'status:', self.waypoint_execution_status, 'executed_index:', executed_waypoint_index, 'success_num:', success_num if self.waypoint_execution_status == 3: # waypoint successfully printed # self.print_list_visualize(point_list[:success_num]) self.rviz_visualise_marker(point_list[:success_num]) del (point_list[:success_num - 1]) elif self.waypoint_execution_status == 2 or 4: # state 2 = preempted, state 4 = aborted. # only printed partial waypoint # self.print_list_visualize(point_list[:executed_waypoint_index+1]) if executed_waypoint_index > 0: # at index 0, it might have not print the point 0-1 edge successfully. self.rviz_visualise_marker( point_list[:executed_waypoint_index + 1]) del (point_list[:executed_waypoint_index] ) # delete up till whatever is executed self.msg_extrude = 0.0 self.extruder_publisher.publish(self.msg_extrude) self.group.stop() # Delete the points what already execute # if success_num > 0: # Add obstacle after printing (need to revise) self.group.set_path_constraints(None) print 'all finish'
sv = StateValidity() init_time = time.time() #rospy.Time.now() result_sv = sv.getStateValidity(rs, "right_arm") fin_time = time.time() #rospy.Time.now() #rospy.loginfo("Call took: " + str((fin_time - init_time).secs) + "s " + str((fin_time - init_time).nsecs)) rospy.loginfo("Call took: " + str(fin_time - init_time)) rospy.loginfo("Result of state validity of current pose (must be valid):" + str(result_sv)) exit(0) c = Constraints() jc = JointConstraint() jc.joint_name = 'arm_right_1_link' jc.position = 0.0 jc.tolerance_above = 0.00001 jc.tolerance_below = 0.00001 jc.weight = 1.0 c.joint_constraints.append(jc) rospy.loginfo("Result without constraints:") resultik = ik.getIK("right_arm", "right_arm_7_link", result.pose_stamped[0], False, 0, rs) rospy.loginfo(str(resultik)) rs = RobotState() rs.joint_state = resultik.solution.joint_state drs = DisplayRobotState() drs.state = rs rs_pub.publish(drs)
def main(): # Initialize the node rospy.init_node("moveit_client") # Create the SimpleActionClient, passing the type of the action # (MoveGroupAction) to the constructor. client = actionlib.SimpleActionClient("move_group", MoveGroupAction) # Wait until the action server has started up and started # listening for goals. client.wait_for_server() # Creates a goal to send to the action server. goal = MoveGroupGoal() # ----------------Construct the goal message (start)---------------- joint_names = [ "head_pan", "left_s0", "left_s1", "left_e0", "left_e1", "left_w0", "left_w1", "left_w2", "right_s0", "right_s1", "right_e0", "right_e1", "right_w0", "right_w1", "right_w2", ] # Set parameters for the planner goal.request.group_name = "both_arms" goal.request.num_planning_attempts = 1 goal.request.allowed_planning_time = 5.0 # Define the workspace in which the planner will search for solutions goal.request.workspace_parameters.min_corner.x = -1 goal.request.workspace_parameters.min_corner.y = -1 goal.request.workspace_parameters.min_corner.z = -1 goal.request.workspace_parameters.max_corner.x = 1 goal.request.workspace_parameters.max_corner.y = 1 goal.request.workspace_parameters.max_corner.z = 1 goal.request.start_state.joint_state.header.frame_id = "base" # Set the start state for the trajectory goal.request.start_state.joint_state.name = joint_names goal.request.start_state.joint_state.position = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ] goal.request.start_state.joint_state.velocity = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, ] # Tell MoveIt whether to execute the trajectory after planning it goal.planning_options.plan_only = True # Set the goal position of the robot # Note that the goal is specified with a collection of individual # joint constraints, rather than a vector of joint angles arm_joint_names = joint_names[1:] print("Joint angles...? Are coming one at a time. Give them to me in radians") target_joint_angles = [] for joint in joint_names: str = raw_input(joint + "\n-->") val = float(str) target_joint_angles.append(val) print("I think that you want me to go to...") print(target_joint_angles) raw_input("Press any key to continue") # target_joint_angles = [0.5, -0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] tolerance = 0.0001 consts = [] for i in range(len(arm_joint_names)): const = JointConstraint() const.joint_name = arm_joint_names[i] const.position = target_joint_angles[i] const.tolerance_above = tolerance const.tolerance_below = tolerance const.weight = 1.0 consts.append(const) goal.request.goal_constraints.append(Constraints(name="", joint_constraints=consts)) # ---------------Construct the goal message (end)----------------- # Send the goal to the action server. client.send_goal(goal) # Wait for the server to finish performing the action. client.wait_for_result() # Print out the result of executing the action print(client.get_result())
def kinect_planner(): global fresh_data global joint_target, pose_target, goal global joints_req, pose_req moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('kinect_trajectory_planner', anonymous=True) rate = rospy.Rate(0.5) #rospy.sleep(3) # Instantiate a RobotCommander object. robot = moveit_commander.RobotCommander() # Instantiate a PlanningSceneInterface object (interface to the world surrounding the robot). scene = moveit_commander.PlanningSceneInterface() print "Remember to disable firewall if it's not working" # Instantiate MoveGroupCommander objects for arms and Kinect2. group = moveit_commander.MoveGroupCommander("Kinect2_Target") group_left_arm = moveit_commander.MoveGroupCommander("left_arm") group_right_arm = moveit_commander.MoveGroupCommander("right_arm") # We create this DisplayTrajectory publisher to publish # trajectories for RVIZ to visualize. display_trajectory_publisher = rospy.Publisher('planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=5) # Set the planner for Moveit group.set_planner_id("RRTConnectkConfigDefault") group.set_planning_time(8) group.set_pose_reference_frame('base_footprint') # Setting tolerance group.set_goal_tolerance(0.08) group.set_num_planning_attempts(10) # Suscribing to the desired pose topic target = rospy.Subscriber("desired_pose", PoseStamped, callback) target_joint = rospy.Subscriber("desired_joints", pose_w_joints, callback_joints) # Locating the arms and the Kinect2 sensor group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() #group_kinect_values = group_kinect.get_current_joint_values() neck_init_joints = group.get_current_joint_values() neck_init_joints[0] = -1.346 neck_init_joints[1] = -1.116 neck_init_joints[2] = -2.121 neck_init_joints[3] = 0.830 neck_init_joints[4] = 1.490 neck_init_joints[5] = 0.050 neck_init_joints[6] = 0 neck_init_joints[7] = 0 neck_init_joints[8] = 0 neck_init_joints[9] = 0 # Creating a box to limit the arm position box_pose = PoseStamped() box_pose.pose.orientation.w = 1 box_pose.pose.position.x = 0.6 box_pose.pose.position.y = 0.03 box_pose.pose.position.z = 1.5 box_pose.header.frame_id = 'base_footprint' scene.add_box('box1', box_pose, (0.4, 0.4, 0.1)) rospy.sleep(2) # Defining position constraints for the trajectory of the kinect neck_const = Constraints() neck_const.name = 'neck_constraints' target_const = JointConstraint() target_const.joint_name = "neck_joint_end" target_const.position = 0.95 target_const.tolerance_above = 0.45 target_const.tolerance_below = 0.05 target_const.weight = 0.9 # Importance of this constraint neck_const.joint_constraints.append(target_const) # Talking to the robot #client = actionlib.SimpleActionClient('/Kinect2_Target_controller/follow_joint_trajectory', FollowJointTrajectoryAction) client = actionlib.SimpleActionClient('/neck/follow_joint_trajectory', FollowJointTrajectoryAction) print "====== Waiting for server..." client.wait_for_server() print "====== Connected to server" while not rospy.is_shutdown(): print('.') skip_iter = False if fresh_data == True: #If a new pose is received, plan. # Update arms position group_left_arm_values = group_left_arm.get_current_joint_values() group_right_arm_values = group_right_arm.get_current_joint_values() # Set the target pose (or joint values) and generate a plan if pose_req: group.set_pose_target(pose_target) if joints_req: print('about to set joint target') print(joint_target) try: if len(joint_target) == 6: #Setting also values for the four virtual joints (one prismatic, and three rotational) new_joint_target = joint_target + [0.1]*4 else: new_joint_target = joint_target print('setting: '), print new_joint_target group.set_joint_value_target(new_joint_target) except: print('cannot set joint target') skip_iter = True print "=========== Calculating trajectory... \n" # Generate several plans and compare them to get the shorter one plan_opt = dict() differ = dict() if not skip_iter: try: num = 1 rep = 0 # Generate several plans and compare them to get the shortest one #for num in range(1,8): while num < 7: num += 1 plan_temp = group.plan() move(plan_temp) plan_opt[num] = goal.trajectory diff=0 for point in goal.trajectory.points: # calculate the distance between initial pose and planned one for i in range(0,6): diff = abs(neck_init_joints[i] - point.positions[i])+abs(diff) differ[num] = diff # If the current plan is good, take it if diff < 110: break # If plan is too bad, don't consider it if diff > 400: num = num - 1 print "Plan is too long. Replanning." rep = rep + 1 if rep > 4: num = num +1 # If no plan was found... if differ == {}: print "---- Fail: No motion plan found. No execution attempted. Probably robot joints are out of range." break else: # Select the shortest plan min_plan = min(differ.itervalues()) select = [k for k, value in differ.iteritems() if value == min_plan] goal.trajectory = plan_opt[select[0]] #print " Plan difference:======= ", differ #print " Selected plan:========= ", select[0] # Remove the last 4 names and data from each point (dummy joints) before sending the goal goal.trajectory.joint_names = goal.trajectory.joint_names[:6] for point in goal.trajectory.points: point.positions = point.positions[:6] point.velocities = point.velocities[:6] point.accelerations = point.accelerations[:6] print "Sending goal" client.send_goal(goal) print "Waiting for result" client.wait_for_result() # Change the position of the virtual joint to avoid collision neck_joints = group.get_current_joint_values() print('neck joints:') print neck_joints #neck_joints[6] = 0.7 #group.set_joint_value_target(neck_joints) #group.go(wait=True) except (KeyboardInterrupt, SystemExit): client.cancel_goal() raise rate.sleep() fresh_data = False rate.sleep()
def main(argv): # root_dir = rospy.get_param("root_dir", default="") root_dir = "/home/user/catkin_ws/src/zipsa_robot_simulation/living_lab_robot_perception" plan_client = actionlib.SimpleActionClient('/plan_and_execute_pose', PlanExecutePoseAction) plan_client.wait_for_server() constrained_plan_client = actionlib.SimpleActionClient( '/plan_and_execute_pose_w_joint_constraints', PlanExecutePoseConstraintsAction) constrained_plan_client.wait_for_server() pose_client = actionlib.SimpleActionClient('/pose_estimation', PoseEstimationAction) pose_client.wait_for_server() rospy.wait_for_service( '/clear_octomap' ) #this will stop your code until the clear octomap service starts running clear_octomap = rospy.ServiceProxy('/clear_octomap', Empty) # clear_octomap() # exit(0) remove_switching_pub = rospy.Publisher('/remove_points_switch', String, queue_size=1) goal = PlanExecutePoseGoal() sub_goal = PlanExecutePoseGoal() tf_buffer = tf2_ros.Buffer() br = tf2_ros.TransformBroadcaster() listener = tf.TransformListener() r = rospy.Rate(10) """ "004_sugar_box", # 1 "006_mustard_bottle", # 2 "005_tomato_soup_can", # 3 "024_bowl", # 4 "water_bottle", # 5 "coca_cola" # 6 """ target_id = 2 initial_poses = np.load("{0}/predefined_poses/{1}.npy".format( root_dir, target_id)) pose_goal = PoseEstimationGoal() pose_goal.target_id = target_id pose_client.send_goal(pose_goal) pose_client.wait_for_result() estimated_pose = pose_client.get_result().estimated_pose estimated_pose = np.reshape(estimated_pose, (4, 4)) print(estimated_pose) for initial_pose in initial_poses: remove_switching_pub.publish("off") print("initial_pose") print(initial_pose) # if initial_pose[2,3] < 0.12: # continue # if initial_pose[2,3] > 0.9: # continue # if initial_pose[0,3] < 0: # continue if np.sum(initial_pose) == 0: continue M = np.matmul(estimated_pose, initial_pose) quat = quaternion_from_matrix(M) try: detect_pose = geometry_msgs.msg.TransformStamped() detect_pose.header.frame_id = "camera_depth_optical_frame" detect_pose.header.stamp = rospy.Time.now() detect_pose.child_frame_id = "object_coordinate" detect_pose.transform.translation.x = M[0, 3] detect_pose.transform.translation.y = M[1, 3] detect_pose.transform.translation.z = M[2, 3] detect_pose.transform.rotation.x = quat[0] detect_pose.transform.rotation.y = quat[1] detect_pose.transform.rotation.z = quat[2] detect_pose.transform.rotation.w = quat[3] count = 0 while 1: count += 1 detect_pose.header.stamp = rospy.Time.now() br.sendTransform(detect_pose) r.sleep() try: (trans, rot) = listener.lookupTransform('/base_footprint', '/object_coordinate', rospy.Time(0)) break except: continue # print("trans: ", trans) # print("rot: ", rot) # convert detect_pose with reference base_footprint goal.target_pose.header.frame_id = "base_footprint" goal.target_pose.pose.position.x = trans[0] goal.target_pose.pose.position.y = trans[1] goal.target_pose.pose.position.z = trans[2] goal.target_pose.pose.orientation.x = rot[0] goal.target_pose.pose.orientation.y = rot[1] goal.target_pose.pose.orientation.z = rot[2] goal.target_pose.pose.orientation.w = rot[3] roll, pitch, yaw = euler_from_quaternion([ goal.target_pose.pose.orientation.x, goal.target_pose.pose.orientation.y, goal.target_pose.pose.orientation.z, goal.target_pose.pose.orientation.w ]) # print(roll, pitch, yaw) if yaw < -math.pi / 2.0: yaw += math.pi elif yaw > math.pi / 2.0: yaw -= math.pi quat = quaternion_from_euler(roll, pitch, yaw) goal.target_pose.pose.orientation.x = quat[0] goal.target_pose.pose.orientation.y = quat[1] goal.target_pose.pose.orientation.z = quat[2] goal.target_pose.pose.orientation.w = quat[3] R = quaternion_matrix(quat)[:3, :3] XL2 = np.matmul(R.T, trans) XL2[0] -= 0.1 sub_trans = np.matmul(R, XL2) sub_goal.target_pose.header.frame_id = "base_footprint" sub_goal.target_pose.pose.position.x = sub_trans[0] sub_goal.target_pose.pose.position.y = sub_trans[1] sub_goal.target_pose.pose.position.z = sub_trans[2] sub_goal.target_pose.pose.orientation.x = quat[0] sub_goal.target_pose.pose.orientation.y = quat[1] sub_goal.target_pose.pose.orientation.z = quat[2] sub_goal.target_pose.pose.orientation.w = quat[3] except ValueError: quit() print(sub_goal) plan_client.send_goal(sub_goal) plan_client.wait_for_result() if plan_client.get_result().result == True: remove_switching_pub.publish("on") rospy.loginfo("Clearing Octomap") clear_octomap() rospy.wait_for_service('/arm_controller/query_state') try: query_state = rospy.ServiceProxy('/arm_controller/query_state', QueryTrajectoryState) resp = query_state(rospy.Time.now()) except rospy.ServiceException, e: print "Service call failed: %s" % e constrained_plan_goal = PlanExecutePoseConstraintsGoal() constrained_plan_goal.target_pose = goal.target_pose joint_names = resp.name joint_positions = resp.position joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm1_joint' # joint_names[2] joint_constraint.position = joint_positions[2] joint_constraint.tolerance_above = (np.pi / 6.0) joint_constraint.tolerance_below = (np.pi / 6.0) joint_constraint.weight = 1.0 constrained_plan_goal.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm4_joint' # joint_names[5] joint_constraint.position = joint_positions[5] joint_constraint.tolerance_above = (np.pi / 6.0) joint_constraint.tolerance_below = (np.pi / 6.0) joint_constraint.weight = 1.0 constrained_plan_goal.joint_constraints.append(joint_constraint) joint_constraint = JointConstraint() joint_constraint.joint_name = 'arm6_joint' # joint_names[7] joint_constraint.position = joint_positions[7] joint_constraint.tolerance_above = (np.pi / 6.0) joint_constraint.tolerance_below = (np.pi / 6.0) joint_constraint.weight = 1.0 constrained_plan_goal.joint_constraints.append(joint_constraint) constrained_plan_client.send_goal(constrained_plan_goal) constrained_plan_client.wait_for_result() print constrained_plan_client.get_result() if constrained_plan_client.get_result().result == True: break else: # Control the manipulator back to the home position. client = actionlib.SimpleActionClient( '/plan_and_execute_named_pose', PlanExecuteNamedPoseAction) client.wait_for_server() named_goal = PlanExecuteNamedPoseGoal() named_goal.target_name = "home" client.send_goal(named_goal) client.wait_for_result() print("=" * 50)