def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): raw_input('Press [ Enter ]: ') #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "right_arm" request.ik_request.ik_link_name = "right_gripper" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = 0.5 request.ik_request.pose_stamped.pose.position.y = 0.5 request.ik_request.pose_stamped.pose.position.z = 0.0 request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) #Print the response HERE print(response) group = MoveGroupCommander("right_arm") # Setting position and orientation target group.set_pose_target(request.ik_request.pose_stamped) # TRY THIS # Setting just the position without specifying the orientation group.set_position_target([0.5, 0.5, 0.0]) # Plan IK and execute group.go() except rospy.ServiceException, e: print "Service call failed: %s" % e
def main(): #Wait for the IK service to become available rospy.wait_for_service('compute_ik') rospy.init_node('service_query') #Create the function used to call the service compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) while not rospy.is_shutdown(): raw_input('Press [ Enter ]: ') #Construct the request request = GetPositionIKRequest() request.ik_request.group_name = "left_arm" request.ik_request.ik_link_name = "left_hand" request.ik_request.attempts = 20 request.ik_request.pose_stamped.header.frame_id = "base" #Set the desired orientation for the end effector HERE request.ik_request.pose_stamped.pose.position.x = 0.5 request.ik_request.pose_stamped.pose.position.y = 0.5 request.ik_request.pose_stamped.pose.position.z = 0.3 request.ik_request.pose_stamped.pose.orientation.x = 0.0 request.ik_request.pose_stamped.pose.orientation.y = 1.0 request.ik_request.pose_stamped.pose.orientation.z = 0.0 request.ik_request.pose_stamped.pose.orientation.w = 0.0 try: #Send the request to the service response = compute_ik(request) #Print the response HERE print(response) group = MoveGroupCommander("left_arm") #this command tries to achieve a pose: which is position, orientation group.set_pose_target(request.ik_request.pose_stamped) #then, this command tries to achieve a position only. orientation is airbitrary. group.set_position_target([0.5,0.5,0.3]) group.go() except rospy.ServiceException, e: print "Service call failed: %s"%e
def __init__(self): moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('cobra_move_position', anonymous=True) arm = MoveGroupCommander(ARM_GROUP) arm.allow_replanning(True) arm.set_planning_time(10) arm.set_named_target(START_POSITION) arm.go() rospy.sleep(2) print "======== create new goal position ========" start_pose = PoseStamped() start_pose.header.frame_id = arm.get_planning_frame() # Test Position start_pose.pose.position.x = 0.2 # 20 cm start_pose.pose.position.y = -0.11 # -11 cm start_pose.pose.position.z = 0.6 # 60 cm q = quaternion_from_euler(0.0, 0.0, 0.0) start_pose.pose.orientation = Quaternion(*q) print start_pose print "====== move to position =======" # KDL # arm.set_joint_value_target(start_pose, True) # IK Fast arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z], arm.get_end_effector_link()) arm.go() rospy.sleep(2) moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0)
class SrRobotCommander(object): """ Base class for hand and arm commanders """ def __init__(self, name): """ Initialize MoveGroupCommander object @param name - name of the MoveIt group """ self._name = name self._move_group_commander = MoveGroupCommander(name) self._robot_commander = RobotCommander() self._robot_name = self._robot_commander._r.get_robot_name() self.refresh_named_targets() self._warehouse_name_get_srv = rospy.ServiceProxy( "get_robot_state", GetState) self._planning_scene = PlanningSceneInterface() self._joint_states_lock = threading.Lock() self._joint_states_listener = \ rospy.Subscriber("joint_states", JointState, self._joint_states_callback, queue_size=1) self._joints_position = {} self._joints_velocity = {} self._joints_effort = {} self._joints_state = None self._clients = {} self.__plan = None self._controllers = {} rospy.wait_for_service('compute_ik') self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK) controller_list_param = rospy.get_param("/move_group/controller_list") # create dictionary with name of controllers and corresponding joints self._controllers = { item["name"]: item["joints"] for item in controller_list_param } self._set_up_action_client(self._controllers) self.tf_buffer = tf2_ros.Buffer() self.listener = tf2_ros.TransformListener(self.tf_buffer) threading.Thread(None, rospy.spin) def set_planner_id(self, planner_id): self._move_group_commander.set_planner_id(planner_id) def set_num_planning_attempts(self, num_planning_attempts): self._move_group_commander.set_num_planning_attempts( num_planning_attempts) def set_planning_time(self, seconds): self._move_group_commander.set_planning_time(seconds) def get_end_effector_pose_from_named_state(self, name): state = self._warehouse_name_get_srv(name, self._robot_name).state return self.get_end_effector_pose_from_state(state) def get_end_effector_pose_from_state(self, state): header = Header() fk_link_names = [self._move_group_commander.get_end_effector_link()] header.frame_id = self._move_group_commander.get_pose_reference_frame() response = self._forward_k(header, fk_link_names, state) return response.pose_stamped[0] def get_planning_frame(self): return self._move_group_commander.get_planning_frame() def set_pose_reference_frame(self, reference_frame): self._move_group_commander.set_pose_reference_frame(reference_frame) def get_group_name(self): return self._name def refresh_named_targets(self): self._srdf_names = self.__get_srdf_names() self._warehouse_names = self.__get_warehouse_names() def set_max_velocity_scaling_factor(self, value): self._move_group_commander.set_max_velocity_scaling_factor(value) def set_max_acceleration_scaling_factor(self, value): self._move_group_commander.set_max_acceleration_scaling_factor(value) def allow_looking(self, value): self._move_group_commander.allow_looking(value) def allow_replanning(self, value): self._move_group_commander.allow_replanning(value) def execute(self): """ Executes the last plan made. """ if self.check_plan_is_valid(): self._move_group_commander.execute(self.__plan) self.__plan = None else: rospy.logwarn("No plans were made, not executing anything.") def execute_plan(self, plan): if self.check_given_plan_is_valid(plan): self._move_group_commander.execute(plan) self.__plan = None else: rospy.logwarn("Plan is not valid, not executing anything.") def move_to_joint_value_target(self, joint_states, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self._move_group_commander.go(wait=wait) def plan_to_joint_value_target(self, joint_states, angle_degrees=False): """ Set target of the robot's links and plans. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param angle_degrees - are joint_states in degrees or not This is a blocking method. """ joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_joint_value_target(joint_states_cpy) self.__plan = self._move_group_commander.plan() return self.__plan def check_plan_is_valid(self): """ Checks if current plan contains a valid trajectory """ return (self.__plan is not None and len(self.__plan.joint_trajectory.points) > 0) def check_given_plan_is_valid(self, plan): """ Checks if given plan contains a valid trajectory """ return (plan is not None and len(plan.joint_trajectory.points) > 0) def get_robot_name(self): return self._robot_name def named_target_in_srdf(self, name): return name in self._srdf_names def set_named_target(self, name): if name in self._srdf_names: self._move_group_commander.set_named_target(name) elif (name in self._warehouse_names): response = self._warehouse_name_get_srv(name, self._robot_name) active_names = self._move_group_commander._g.get_active_joints() joints = response.state.joint_state.name positions = response.state.joint_state.position js = {} for n, this_name in enumerate(joints): if this_name in active_names: js[this_name] = positions[n] self._move_group_commander.set_joint_value_target(js) else: rospy.logerr("Unknown named state '%s'..." % name) return False return True def get_named_target_joint_values(self, name): output = dict() if (name in self._srdf_names): output = self._move_group_commander.\ _g.get_named_target_values(str(name)) elif (name in self._warehouse_names): js = self._warehouse_name_get_srv( name, self._robot_name).state.joint_state for x, n in enumerate(js.name): if n in self._move_group_commander._g.get_joints(): output[n] = js.position[x] else: rospy.logerr("No target named %s" % name) return None return output def get_end_effector_link(self): return self._move_group_commander.get_end_effector_link() def get_current_pose(self, reference_frame=None): """ Get the current pose of the end effector. @param reference_frame - The desired reference frame in which end effector pose should be returned. If none is passed, it will use the planning frame as reference. @return geometry_msgs.msg.Pose() - current pose of the end effector """ if reference_frame is not None: try: trans = self.tf_buffer.lookup_transform( reference_frame, self._move_group_commander.get_end_effector_link(), rospy.Time(0), rospy.Duration(5.0)) current_pose = geometry_msgs.msg.Pose() current_pose.position.x = trans.transform.translation.x current_pose.position.y = trans.transform.translation.y current_pose.position.z = trans.transform.translation.z current_pose.orientation.x = trans.transform.rotation.x current_pose.orientation.y = trans.transform.rotation.y current_pose.orientation.z = trans.transform.rotation.z current_pose.orientation.w = trans.transform.rotation.w return current_pose except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): rospy.logwarn( "Couldn't get the pose from " + self._move_group_commander.get_end_effector_link() + " in " + reference_frame + " reference frame") return None else: return self._move_group_commander.get_current_pose().pose def get_current_state(self): """ Get the current joint state of the group being used. @return a dictionary with the joint names as keys and current joint values """ joint_names = self._move_group_commander._g.get_active_joints() joint_values = self._move_group_commander._g.get_current_joint_values() return dict(zip(joint_names, joint_values)) def get_current_state_bounded(self): """ Get the current joint state of the group being used, enforcing that they are within each joint limits. @return a dictionary with the joint names as keys and current joint values """ current = self._move_group_commander._g.get_current_state_bounded() names = self._move_group_commander._g.get_active_joints() output = {n: current[n] for n in names if n in current} return output def get_robot_state_bounded(self): return self._move_group_commander._g.get_current_state_bounded() def move_to_named_target(self, name, wait=True): """ Set target of the robot's links and moves to it @param name - name of the target pose defined in SRDF @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self._move_group_commander.go(wait=wait) def plan_to_named_target(self, name): """ Set target of the robot's links and plans This is a blocking method. @param name - name of the target pose defined in SRDF """ self._move_group_commander.set_start_state_to_current_state() if self.set_named_target(name): self.__plan = self._move_group_commander.plan() def __get_warehouse_names(self): try: list_srv = rospy.ServiceProxy("list_robot_states", ListStates) return list_srv("", self._robot_name).states except rospy.ServiceException as exc: rospy.logwarn("Couldn't access warehouse: " + str(exc)) return list() def _reset_plan(self): self.__plan = None def _set_plan(self, plan): self.__plan = plan def __get_srdf_names(self): return self._move_group_commander._g.get_named_targets() def get_named_targets(self): """ Get the complete list of named targets, from SRDF as well as warehouse poses if available. @return list of strings containing names of targets. """ return self._srdf_names + self._warehouse_names def get_joints_position(self): """ Returns joints position @return - dictionary with joints positions """ with self._joint_states_lock: return self._joints_position def get_joints_velocity(self): """ Returns joints velocities @return - dictionary with joints velocities """ with self._joint_states_lock: return self._joints_velocity def _get_joints_effort(self): """ Returns joints effort @return - dictionary with joints efforts """ with self._joint_states_lock: return self._joints_effort def get_joints_state(self): """ Returns joints state @return - JointState message """ with self._joint_states_lock: return self._joints_state def run_joint_trajectory(self, joint_trajectory): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. """ plan = RobotTrajectory() plan.joint_trajectory = joint_trajectory self._move_group_commander.execute(plan) def make_named_trajectory(self, trajectory): """ Makes joint value trajectory from specified by named poses (either from SRDF or from warehouse) @param trajectory - list of waypoints, each waypoint is a dict with the following elements (n.b either name or joint_angles is required) - name -> the name of the way point - joint_angles -> a dict of joint names and angles - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent. """ current = self.get_current_state_bounded() joint_trajectory = JointTrajectory() joint_names = current.keys() joint_trajectory.joint_names = joint_names start = JointTrajectoryPoint() start.positions = current.values() start.time_from_start = rospy.Duration.from_sec(0.001) joint_trajectory.points.append(start) time_from_start = 0.0 for wp in trajectory: joint_positions = None if 'name' in wp.keys(): joint_positions = self.get_named_target_joint_values( wp['name']) elif 'joint_angles' in wp.keys(): joint_positions = copy.deepcopy(wp['joint_angles']) if 'degrees' in wp.keys() and wp['degrees']: for joint, angle in joint_positions.iteritems(): joint_positions[joint] = radians(angle) if joint_positions is None: rospy.logerr( "Invalid waypoint. Must contain valid name for named target or dict of joint angles." ) return None new_positions = {} for n in joint_names: new_positions[n] = joint_positions[ n] if n in joint_positions else current[n] trajectory_point = JointTrajectoryPoint() trajectory_point.positions = [ new_positions[n] for n in joint_names ] current = new_positions time_from_start += wp['interpolate_time'] trajectory_point.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(trajectory_point) if 'pause_time' in wp and wp['pause_time'] > 0: extra = JointTrajectoryPoint() extra.positions = trajectory_point.positions time_from_start += wp['pause_time'] extra.time_from_start = rospy.Duration.from_sec( time_from_start) joint_trajectory.points.append(extra) return joint_trajectory def send_stop_trajectory_unsafe(self): """ Sends a trajectory of all active joints at their current position. This stops the robot. """ current = self.get_current_state_bounded() trajectory_point = JointTrajectoryPoint() trajectory_point.positions = current.values() trajectory_point.time_from_start = rospy.Duration.from_sec(0.1) trajectory = JointTrajectory() trajectory.points.append(trajectory_point) trajectory.joint_names = current.keys() self.run_joint_trajectory_unsafe(trajectory) def run_named_trajectory_unsafe(self, trajectory, wait=False): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory directly via contoller. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory_unsafe(joint_trajectory, wait) def run_named_trajectory(self, trajectory): """ Moves robot through trajectory specified by named poses, either from SRDF or from warehouse. Runs trajectory via moveit. @param trajectory - list of waypoints, each waypoint is a dict with the following elements: - name -> the name of the way point - interpolate_time -> time to move from last wp - pause_time -> time to wait at this wp """ joint_trajectory = self.make_named_trajectory(trajectory) if joint_trajectory is not None: self.run_joint_trajectory(joint_trajectory) def move_to_position_target(self, xyz, end_effector_link="", wait=True): """ Specify a target position for the end-effector and moves to it @param xyz - new position of end-effector @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_position_target(self, xyz, end_effector_link=""): """ Specify a target position for the end-effector and plans. This is a blocking method. @param xyz - new position of end-effector @param end_effector_link - name of the end effector link """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_position_target(xyz, end_effector_link) self.__plan = self._move_group_commander.plan() def move_to_pose_target(self, pose, end_effector_link="", wait=True): """ Specify a target pose for the end-effector and moves to it @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param wait - should method wait for movement end or not """ self._move_group_commander.set_start_state_to_current_state() self._move_group_commander.set_pose_target(pose, end_effector_link) self._move_group_commander.go(wait=wait) def plan_to_pose_target(self, pose, end_effector_link="", alternative_method=False): """ Specify a target pose for the end-effector and plans. This is a blocking method. @param pose - new pose of end-effector: a Pose message, a PoseStamped message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list of 7 floats [x, y, z, qx, qy, qz, qw] @param end_effector_link - name of the end effector link @param alternative_method - use set_joint_value_target instead of set_pose_target """ self._move_group_commander.set_start_state_to_current_state() if alternative_method: self._move_group_commander.set_joint_value_target( pose, end_effector_link) else: self._move_group_commander.set_pose_target(pose, end_effector_link) self.__plan = self._move_group_commander.plan() return self.__plan def _joint_states_callback(self, joint_state): """ The callback function for the topic joint_states. It will store the received joint position, velocity and efforts information into dictionaries @param joint_state - the message containing the joints data. """ with self._joint_states_lock: self._joints_state = joint_state self._joints_position = { n: p for n, p in zip(joint_state.name, joint_state.position) } self._joints_velocity = { n: v for n, v in zip(joint_state.name, joint_state.velocity) } self._joints_effort = { n: v for n, v in zip(joint_state.name, joint_state.effort) } def _set_up_action_client(self, controller_list): """ Sets up an action client to communicate with the trajectory controller """ self._action_running = {} for controller_name in controller_list.keys(): self._action_running[controller_name] = False service_name = controller_name + "/follow_joint_trajectory" self._clients[controller_name] = SimpleActionClient( service_name, FollowJointTrajectoryAction) if self._clients[controller_name].wait_for_server( timeout=rospy.Duration(4)) is False: err_msg = 'Failed to connect to action server ({}) in 4 sec'.format( service_name) rospy.logwarn(err_msg) def move_to_joint_value_target_unsafe(self, joint_states, time=0.002, wait=True, angle_degrees=False): """ Set target of the robot's links and moves to it. @param joint_states - dictionary with joint name and value. It can contain only joints values of which need to be changed. @param time - time in s (counting from now) for the robot to reach the target (it needs to be greater than 0.0 for it not to be rejected by the trajectory controller) @param wait - should method wait for movement end or not @param angle_degrees - are joint_states in degrees or not """ # self._update_default_trajectory() # self._set_targets_to_default_trajectory(joint_states) goals = {} joint_states_cpy = copy.deepcopy(joint_states) if angle_degrees: joint_states_cpy.update( (joint, radians(i)) for joint, i in joint_states_cpy.items()) for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory.joint_names = [] point = JointTrajectoryPoint() point.positions = [] for x in joint_states_cpy.keys(): if x in controller_joints: goal.trajectory.joint_names.append(x) point.positions.append(joint_states_cpy[x]) point.time_from_start = rospy.Duration.from_sec(time) goal.trajectory.points = [point] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def action_is_running(self, controller=None): if controller is not None: return self._action_running[controller] for controller_running in self._action_running.values(): if controller_running: return True return False def _action_done_cb(self, controller, terminal_state, result): self._action_running[controller] = False def _call_action(self, goals): for client in self._clients: self._action_running[client] = True self._clients[client].send_goal( goals[client], lambda terminal_state, result: self._action_done_cb( client, terminal_state, result)) def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True): """ Moves robot through all joint states with specified timeouts @param joint_trajectory - JointTrajectory class object. Represents trajectory of the joints which would be executed. @param wait - should method wait for movement end or not """ goals = {} for controller in self._controllers: controller_joints = self._controllers[controller] goal = FollowJointTrajectoryGoal() goal.trajectory = copy.deepcopy(joint_trajectory) indices_of_joints_in_this_controller = [] for i, joint in enumerate(joint_trajectory.joint_names): if joint in controller_joints: indices_of_joints_in_this_controller.append(i) goal.trajectory.joint_names = [ joint_trajectory.joint_names[i] for i in indices_of_joints_in_this_controller ] for point in goal.trajectory.points: if point.positions: point.positions = [ point.positions[i] for i in indices_of_joints_in_this_controller ] if point.velocities: point.velocities = [ point.velocities[i] for i in indices_of_joints_in_this_controller ] if point.effort: point.effort = [ point.effort[i] for i in indices_of_joints_in_this_controller ] goals[controller] = goal self._call_action(goals) if not wait: return for i in self._clients.keys(): if not self._clients[i].wait_for_result(): rospy.loginfo("Trajectory not completed") def plan_to_waypoints_target(self, waypoints, reference_frame=None, eef_step=0.005, jump_threshold=0.0): """ Specify a set of waypoints for the end-effector and plans. This is a blocking method. @param reference_frame - the reference frame in which the waypoints are given @param waypoints - an array of poses of end-effector @param eef_step - configurations are computed for every eef_step meters @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path """ old_frame = self._move_group_commander.get_pose_reference_frame() if reference_frame is not None: self.set_pose_reference_frame(reference_frame) (self.__plan, fraction) = self._move_group_commander.compute_cartesian_path( waypoints, eef_step, jump_threshold) self.set_pose_reference_frame(old_frame) def set_teach_mode(self, teach): """ Activates/deactivates the teach mode for the robot. Activation: stops the the trajectory controllers for the robot, and sets it to teach mode. Deactivation: stops the teach mode and starts trajectory controllers for the robot. Currently this method blocks for a few seconds when called on a hand, while the hand parameters are reloaded. @param teach - bool to activate or deactivate teach mode """ if teach: mode = RobotTeachModeRequest.TEACH_MODE else: mode = RobotTeachModeRequest.TRAJECTORY_MODE self.change_teach_mode(mode, self._name) def move_to_trajectory_start(self, trajectory, wait=True): """ Make and execute a plan from the current state to the first state in an pre-existing trajectory @param trajectory - moveit_msgs/JointTrajectory @param wait - Bool to specify if movement should block untill finished. """ if len(trajectory.points) <= 0: rospy.logerr("Trajectory has no points in it, can't reverse...") return None first_point = trajectory.points[0] end_state = dict(zip(trajectory.joint_names, first_point.positions)) self.move_to_joint_value_target(end_state, wait=wait) @staticmethod def change_teach_mode(mode, robot): teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode) req = RobotTeachModeRequest() req.teach_mode = mode req.robot = robot try: resp = teach_mode_client(req) if resp.result == RobotTeachModeResponse.ERROR: rospy.logerr("Failed to change robot %s to mode %d", robot, mode) else: rospy.loginfo("Changed robot %s to mode %d Result = %d", robot, mode, resp.result) except rospy.ServiceException: rospy.logerr("Failed to call service teach_mode") def get_ik(self, target_pose, avoid_collisions=False, joint_states=None): """ Computes the inverse kinematics for a given pose. It returns a JointState @param target_pose - A given pose of type PoseStamped @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false """ service_request = PositionIKRequest() service_request.group_name = self._name service_request.ik_link_name = self._move_group_commander.get_end_effector_link( ) service_request.pose_stamped = target_pose service_request.timeout.secs = 0.5 service_request.avoid_collisions = avoid_collisions if joint_states is None: service_request.robot_state.joint_state = self.get_joints_state() else: service_request.robot_state.joint_state = joint_states try: resp = self._compute_ik(ik_request=service_request) # Check if error_code.val is SUCCESS=1 if resp.error_code.val != 1: if resp.error_code.val == -10: rospy.logerr("Unreachable point: Start state in collision") elif resp.error_code.val == -12: rospy.logerr("Unreachable point: Goal state in collision") elif resp.error_code.val == -31: rospy.logerr("Unreachable point: No IK solution") else: rospy.logerr("Unreachable point (error: %s)" % resp.error_code) return else: return resp.solution.joint_state except rospy.ServiceException, e: rospy.logerr("Service call failed: %s" % e)
pose_target_2 = Pose() pose_target_2.position.x = 0.3 pose_target_2.position.y = -0.5 pose_target_2.position.z = 0.15 pose_target_2.orientation.x = 0.0 pose_target_2.orientation.y = -0.707 pose_target_2.orientation.z = 0.0 pose_target_2.orientation.w = 0.707 group.set_planner_id("RRTConnectkConfigDefault") group.allow_replanning(True) rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_2)) xyz = [ pose_target_2.position.x, pose_target_2.position.y, pose_target_2.position.z ] group.set_position_target(xyz) result_p = group.go() rospy.loginfo("Moving to the Position Executed... {}".format(result_p)) group.clear_path_constraints() if result_p: group.set_pose_target(pose_target_2) result_o = group.go() rospy.loginfo( "Adjusting the Orientation Executed... {}".format(result_o))
# arm.set_goal_joint_tolerance(0.01) # arm.set_goal_tolerance(0.01) # Action Clients for Place rospy.loginfo("Connecting to place AS") place_ac = actionlib.SimpleActionClient('/place', PlaceAction) place_ac.wait_for_server() rospy.loginfo("Successfully connected") rospy.sleep(1) # take the part to specific pose pose = generate_rand_position() # IKFast arm.set_position_target([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z], arm.get_end_effector_link()) # KDL # arm.set_joint_value_target(pose, True) arm.go() rospy.sleep(5) # ===== place start ==== # place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose()) rospy.loginfo("Place Result is:") rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val])) rospy.sleep(5) # end moveit_commander.roscpp_shutdown()
#!/usr/bin/env python import rospy from moveit_commander import MoveGroupCommander, RobotCommander, PlanningSceneInterface rospy.init_node("goto_point") robot = RobotCommander() gripper_group = MoveGroupCommander("gripper_group") scene = PlanningSceneInterface() rospy.sleep(2) xyz_goal = [0, 0.0, 1.5] gripper_group.set_position_target(xyz_goal, gripper_group.get_end_effector_link()) gripper_group.go()
gripper_target_1 = [ 0.01, 0] gripper_group.set_joint_value_target(gripper_target_1) gripper_group.go() rospy.sleep(5.0) pose_current = group.get_current_pose() rospy.loginfo( "Get Current Pose:\n{}\n".format( pose_current ) ) # Pose 2 rospy.loginfo( "Starting Pose 2") group.set_position_target([0.2, -0.1, 0.05]) group.go() gripper_target_2 = [ -0.01, 0] gripper_group.set_joint_value_target(gripper_target_2) gripper_group.go() rospy.sleep(5.0) pose_current = group.get_current_pose() rospy.loginfo( "Get Current Pose:\n{}\n".format( pose_current ) ) # Pose 3 rospy.loginfo( "Starting Pose 3") pose_target_3 = Pose()
# arm.set_goal_tolerance(0.01) # Action Clients for Place rospy.loginfo("Connecting to place AS") place_ac = actionlib.SimpleActionClient('/place', PlaceAction) place_ac.wait_for_server() rospy.loginfo("Successfully connected") rospy.sleep(1) # take the part to specific pose pose = generate_rand_position() # IKFast arm.set_position_target( [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z], arm.get_end_effector_link()) # KDL # arm.set_joint_value_target(pose, True) arm.go() rospy.sleep(5) # ===== place start ==== # place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose()) rospy.loginfo("Place Result is:") rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val])) rospy.sleep(5)
class PathPlanner: """ This path planner wraps the planning and actuation functionality provided by MoveIt. All positions are arrays holding x, y, and z coordinates. Orientations are arrays holding x, y, z, and w coordinates (in quaternion form). Attributes: frame_id (str): The frame all coordinates are relative to. workspace (list): The bounds of the planning space (a box). Specified as the minimum x, y, and z coordinates, then the maximum x, y, and z coordinates. group_name (str): The MoveIt group name (for example, 'right_arm'). time_limit (float): The maximum number of seconds MoveIt will plan for. robot: The MoveIt robot commander. scene: The planning scene. group: The MoveIt MoveGroup. scene_publisher: A publisher that updates the planning scene. """ PLANNING_SCENE_TOPIC = '/collision_object' def __init__(self, frame_id, group_name, time_limit=10, workspace=None, register_shutdown=True): if workspace is None: workspace = [-2, -2, -2, 2, 2, 2] if rospy.get_param('verbose'): rospy.loginfo('Move group: {}'.format(group_name)) self.frame_id, self.workspace = frame_id, workspace self.time_limit, self.group_name = time_limit, group_name self.robot = RobotCommander() self.scene = PlanningSceneInterface() self.scene_publisher = rospy.Publisher(self.PLANNING_SCENE_TOPIC, CollisionObject, queue_size=10) self.group = MoveGroupCommander(group_name) if register_shutdown: rospy.on_shutdown(self.shutdown) self.group.set_planning_time(time_limit) self.group.set_workspace(workspace) rospy.sleep(0.5) # Sleep to ensure initialization has finished. if rospy.get_param('verbose'): rospy.loginfo('Initialized path planner.') def shutdown(self): """ Stop the path planner safely. """ self.group.stop() del self.group if rospy.get_param('verbose'): rospy.logwarn('Terminated path planner.') def move_to_pose(self, position, orientation=None): """ Move the end effector to the given pose naively. Arguments: position: The x, y, and z coordinates to move to. orientation: The orientation to take (quaternion, optional). Raises (rospy.ServiceException): Failed to execute movement. """ if orientation is None: self.group.set_position_target(position) else: self.group.set_pose_target(create_pose(self.frame_id, position, orientation)) if rospy.get_param('verbose'): log_pose('Moving to pose.', position, orientation) self.group.go(wait=True) self.group.stop() self.group.clear_pose_targets() def move_to_pose_with_planner(self, position, orientation=None, orientation_constraints=None): """ Move the end effector to the given pose, taking into account constraints and planning scene obstacles. Arguments: position: The x, y, and z coordinates to move to. orientation: The orientation to take (quaternion, optional). orientation_constraints (list): A list of `OrientationConstraint` objects created with `make_orientation_constraint`. Returns (bool): Whether or not the movement executed successfully. """ if orientation_constraints is None: orientation_constraints = [] target = create_pose(self.frame_id, position, orientation) if rospy.get_param('verbose'): log_pose('Moving to pose with planner.', position, orientation) for _ in range(3): try: plan = self.plan_to_pose(target, orientation_constraints) if not self.group.execute(plan, True): raise ValueError('Execution failed.') except ValueError as exc: rospy.logwarn('Failed to perform movement: {}. Retrying.'.format(str(exc))) else: break else: raise ValueError('Failed to move to pose.') def plan_to_pose(self, target, orientation_constraints): """ Plan a movement to a pose from the current state, given constraints. Arguments: target: The destination pose. orientation_constraints (list): The constraints. Returns (moveit_msgs.msg.RobotTrajectory): The path. """ self.group.set_pose_target(target) self.group.set_start_state_to_current_state() constraints = Constraints() constraints.orientation_constraints = orientation_constraints self.group.set_path_constraints(constraints) return self.group.plan() def add_box_obstacle(self, dimensions, name, com_position, com_orientation): """ Add a rectangular prism obstacle to the planning scene. Arguments: dimensions: An array containing the width, length, and height of the box (in the box's body frame, corresponding to x, y, and z). name: A unique name for identifying this obstacle. com_position: The position of the center-of-mass (COM) of this box, relative to the global frame `frame_id`. com_orientation: The orientation of the COM. """ pose = create_pose(self.frame_id, com_position, com_orientation) obj = CollisionObject() obj.id, obj.operation, obj.header = name, CollisionObject.ADD, pose.header box = SolidPrimitive() box.type, box.dimensions = SolidPrimitive.BOX, dimensions obj.primitives, obj.primitive_poses = [box], [pose.pose] self.scene_publisher.publish(obj) if rospy.get_param('verbose'): rospy.loginfo('Added box object "{}" to planning scene: ' '(x={}, y={}, z={}).'.format(name, *dimensions)) def remove_obstacle(self, name): """ Remove a named obstacle from the planning scene. """ obj = CollisionObject() obj.id, obj.operation = name, CollisionObject.REMOVE self.scene_publisher.publish(obj) if rospy.get_param('verbose'): rospy.loginfo('Removed object "{}" from planning scene.'.format(name)) def make_orientation_constraint(self, orientation, link_id, tolerance=0.1, weight=1): """ Make an orientation constraint in the context of the robot and world. Arguments: orientation: The orientation the link should have. link_id: The name of the link frame. Returns (OrientationConstraint): The constraint. """ constraint = OrientationConstraint() constraint.header.frame_id = self.frame_id constraint.link_name = link_id const_orien = constraint.orientation const_orien.x, const_orien.y, const_orien.z, const_orien.w = orientation constraint.absolute_x_axis_tolerance = tolerance constraint.absolute_y_axis_tolerance = tolerance constraint.absolute_z_axis_tolerance = tolerance constraint.weight = weight return constraint
import rospy from moveit_commander import RobotCommander, MoveGroupCommander from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown from geometry_msgs.msg import PoseStamped, Pose from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation from trajectory_msgs.msg import JointTrajectoryPoint rospy.init_node("motion_planning") scene = PlanningSceneInterface() robot = RobotCommander() L_arm = MoveGroupCommander("L_arm") rospy.sleep (1) #print (robot.get_current_variable_values()) print (L_arm.get_current_pose()) print (L_arm.get_planning_frame()) L_arm.set_planning_time(10); L_arm.set_pose_reference_frame("omo_L") L_arm.set_position_target([-0.068, -0.120, 0.520]) print (L_arm.get_end_effector_link()) plan = L_arm.plan() print (plan) L_arm.execute(plan) print (L_arm.get_current_pose(L_arm.get_end_effector_link()))
class PlannerAnnotationParser(AnnotationParserBase): """ Parses the annotations files that contains the benchmarking information. """ def __init__(self, path_to_annotation, path_to_data): super(PlannerAnnotationParser, self).__init__(path_to_annotation, path_to_data) self.parse() self._load_scene() self._init_planning() self.benchmark() def check_results(self, results): """ Returns the results from the planner, checking them against any eventual validation data (no validation data in our case). """ return self.planner_data def _load_scene(self): """ Loads the proper scene for the planner. It can be either a python static scene or bag containing an occupancy map. """ scene = self._annotations["scene"] for element in scene: if element["type"] == "launch": self.play_launch(element["name"]) elif element["type"] == "python": self.load_python(element["name"]) elif element["type"] == "bag": self.play_bag(element["name"]) for _ in range(150): rospy.sleep(0.3) # wait for the scene to be spawned properly rospy.sleep(0.5) def _init_planning(self): """ Initialises the needed connections for the planning. """ self.group_id = self._annotations["group_id"] self.planners = self._annotations["planners"] self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.group = MoveGroupCommander(self.group_id) self._marker_pub = rospy.Publisher('/visualization_marker_array', MarkerArray, queue_size=10, latch=True) self._planning_time_sub = rospy.Subscriber( '/move_group/result', MoveGroupActionResult, self._check_computation_time) rospy.sleep(1) self.group.set_num_planning_attempts( self._annotations["planning_attempts"]) self.group.set_goal_tolerance(self._annotations["goal_tolerance"]) self.group.set_planning_time(self._annotations["planning_time"]) self.group.allow_replanning(self._annotations["allow_replanning"]) self._comp_time = [] self.planner_data = [] def benchmark(self): for test_id, test in enumerate(self._annotations["tests"]): marker_position_1 = test["start_xyz"] marker_position_2 = test["goal_xyz"] self.space = test["space"] self._add_markers(marker_position_1, "Start test \n sequence", marker_position_2, "Goal") # Start planning in a given joint position joints = test["start_joints"] current = RobotState() current.joint_state.name = self.robot.get_current_state( ).joint_state.name current_joints = list( self.robot.get_current_state().joint_state.position) current_joints[0:6] = joints current.joint_state.position = current_joints self.group.set_start_state(current) if self.space == "joint": coordinates = test["goal_joints"] elif self.space == "pose": position = test["goal_xyz"] orientation = test["goal_orientation"] coordinates = self.create_pose(position, orientation) elif self.space == "position": coordinates = test["goal_xyz"] for planner in self.planners: if planner == "stomp": planner = "STOMP" elif planner == "sbpl": planner = "AnytimeD*" self.planner_id = planner self.group.set_planner_id(planner) self._plan_joints( coordinates, self._annotations["name"] + "-test_" + str(test_id) + "-" + self.space) return self.planner_data def create_pose(self, position, orientation): pose = Pose() pose.position.x = position[0] pose.position.y = position[1] pose.position.z = position[2] pose.orientation.x = orientation[0] pose.orientation.y = orientation[1] pose.orientation.z = orientation[2] pose.orientation.w = orientation[3] return pose def _add_markers(self, point, text1, point_2, text2): # add marker for start and goal pose of EE marker_array = MarkerArray() marker_1 = Marker() marker_1.header.frame_id = "world" marker_1.header.stamp = rospy.Time.now() marker_1.type = Marker.SPHERE marker_1.scale.x = 0.04 marker_1.scale.y = 0.04 marker_1.scale.z = 0.04 marker_1.lifetime = rospy.Duration() marker_2 = deepcopy(marker_1) marker_1.color.g = 0.5 marker_1.color.a = 1.0 marker_1.id = 0 marker_1.pose.position.x = point[0] marker_1.pose.position.y = point[1] marker_1.pose.position.z = point[2] marker_2.color.r = 0.5 marker_2.color.a = 1.0 marker_2.id = 1 marker_2.pose.position.x = point_2[0] marker_2.pose.position.y = point_2[1] marker_2.pose.position.z = point_2[2] marker_3 = Marker() marker_3.header.frame_id = "world" marker_3.header.stamp = rospy.Time.now() marker_3.type = Marker.TEXT_VIEW_FACING marker_3.scale.z = 0.10 marker_3.lifetime = rospy.Duration() marker_4 = deepcopy(marker_3) marker_3.text = text1 marker_3.id = 2 marker_3.color.g = 0.5 marker_3.color.a = 1.0 marker_3.pose.position.x = point[0] marker_3.pose.position.y = point[1] marker_3.pose.position.z = point[2] + 0.15 marker_4.text = text2 marker_4.id = 3 marker_4.color.r = 0.5 marker_4.color.a = 1.0 marker_4.pose.position.x = point_2[0] marker_4.pose.position.y = point_2[1] marker_4.pose.position.z = point_2[2] + 0.15 marker_array.markers = [marker_1, marker_2, marker_3, marker_4] self._marker_pub.publish(marker_array) rospy.sleep(1) def _plan_joints(self, joints, test_name): # plan to joint target and determine success self.group.clear_pose_targets() if self.space == "joint": group_variable_values = self.group.get_current_joint_values() group_variable_values[0:6] = joints[0:6] self.group.set_joint_value_target(group_variable_values) elif self.space == "pose": self.group.set_joint_value_target(joints) elif self.space == "position": self.group.set_position_target(joints) plan = self.group.plan() plan_time = "N/A" total_joint_rotation = "N/A" comp_time = "N/A" plan_evaluation = "N/A" plan_success = self._check_plan_success(plan) if plan_success: plan_time = self._check_plan_time(plan) total_joint_rotation = self._check_plan_total_rotation(plan) plan_evaluation = self.evaluate_plan(plan) while not self._comp_time: rospy.sleep(0.5) comp_time = self._comp_time.pop(0) self.planner_data.append([ self.planner_id, test_name, str(plan_success), total_joint_rotation, plan_evaluation, comp_time, plan_time ]) @staticmethod def _check_plan_success(plan): if len(plan.joint_trajectory.points) > 0: return True else: return False @staticmethod def _check_plan_time(plan): # find duration of successful plan number_of_points = len(plan.joint_trajectory.points) time = plan.joint_trajectory.points[number_of_points - 1].time_from_start.to_sec() return time @staticmethod def _check_plan_total_rotation(plan): # find total joint rotation in successful trajectory angles = [0, 0, 0, 0, 0, 0] number_of_points = len(plan.joint_trajectory.points) for i in range(number_of_points - 1): angles_temp = [ abs(x - y) for x, y in zip(plan.joint_trajectory.points[i + 1].positions, plan.joint_trajectory.points[i].positions) ] angles = [x + y for x, y in zip(angles, angles_temp)] total_angle_change = sum(angles) return total_angle_change def evaluate_plan(self, plan): num_of_joints = len(plan.joint_trajectory.points[0].positions) weights = numpy.array(sorted(range(1, num_of_joints + 1), reverse=True)) plan_array = numpy.empty(shape=(len(plan.joint_trajectory.points), num_of_joints)) for i, point in enumerate(plan.joint_trajectory.points): plan_array[i] = point.positions deltas = abs(numpy.diff(plan_array, axis=0)) sum_deltas = numpy.sum(deltas, axis=0) sum_deltas_weighted = sum_deltas * weights plan_quality = numpy.sum(sum_deltas_weighted) return plan_quality def _check_computation_time(self, msg): # get computation time for successful plan to be found if msg.status.status == 3: self._comp_time.append(msg.result.planning_time) def _check_path_length(self, plan): # find distance travelled by end effector # not yet implemented return
class MoveitMove(EventState): ''' Move the arm to a specific pose or point or named_target -- move wetter or not the arm should move or simply check if the move is possible -- waitForExecution wait for execution to end before continuing ># pose Pose Targetpose. <= done No error occurred. <= failed The plan could't be done. ''' def __init__(self, move=True, waitForExecution=True, group="RightArm", watchdog=15): # See example_state.py for basic explanations. super(MoveitMove, self).__init__(outcomes=['done', 'failed'], input_keys=['target']) self.move = move self.waitForExecution = waitForExecution self.group = MoveGroupCommander(group) self.tol = 0.06 self.result = None self.count = 0 self.timeout = rospy.Time.now() self.watchdog = watchdog def execute(self, userdata): if self.result: return self.result if self.waitForExecution: curState = self.group.get_current_joint_values() diff = compareStates(curState, self.endState) print("diff=" + str(diff)) if diff < self.tol or self.timeout + rospy.Duration( self.watchdog) < rospy.Time.now(): self.count += 1 if self.count > 3: Logger.loginfo('Target reached :)') return "done" else: self.count = 0 else: return "done" def on_enter(self, userdata): Logger.loginfo('Enter Move Arm') self.timeout = rospy.Time.now() if type(userdata.target) is Pose: Logger.loginfo('the target is a pose') self.group.set_pose_target(userdata.target) elif type(userdata.target) is Point: Logger.loginfo('the target is a point') xyz = [userdata.target.x, userdata.target.y, userdata.target.z] self.group.set_position_target(xyz) elif type(userdata.target) is str: Logger.loginfo('the target is a named_target') self.group.set_named_target(userdata.target) else: Logger.loginfo( 'ERROR in ' + str(self.name) + ' : target is not a Pose() nor a Point() nor a string') self.result = 'failed' Logger.loginfo('target defined') try: plan = self.group.plan() Logger.loginfo(str(plan)) # Logger.loginfo(str(plan.joint_trajectory.points)) self.endState = plan.joint_trajectory.points[ len(plan.joint_trajectory.points) - 1].positions Logger.loginfo(str(self.endState)) except: Logger.loginfo('Planning failed') self.result = 'failed' return Logger.loginfo('Plan done successfully') if self.move: Logger.loginfo('Initiating movement') self.group.execute(plan, wait=False) if not self.waitForExecution: self.result = "done" else: Logger.loginfo('The target is reachable') self.result = "done" def on_exit(self, userdata): Logger.loginfo('Stoping movement') self.group.stop() def on_pause(self): Logger.loginfo('Pausing movement') self.group.stop() def on_resume(self, userdata): self.on_enter(userdata)