def compute_cartesian_path( self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None, ): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ if path_constraints: if type(path_constraints) is Constraints: constraints_str = conversions.msg_to_string(path_constraints) else: raise MoveItCommanderException( "Unable to set path constraints, unknown constraint type " + type(path_constraints)) (ser_path, fraction) = self._g.compute_cartesian_path( [conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str, ) else: (ser_path, fraction) = self._g.compute_cartesian_path( [conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, ) path = RobotTrajectory() path.deserialize(ser_path) return (path, fraction)
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor): ser_ref_state_in = conversions.msg_to_string(ref_state_in) ser_traj_in = conversions.msg_to_string(traj_in) ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor) traj_out = RobotTrajectory() traj_out.deserialize(ser_traj_out) return traj_out
def runTrajectory(req): global Traj_server; print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin"+req.bin_num); else: file_root = os.path.join(os.path.dirname(__file__), "../../optimize_trajectories/bin"+req.bin_num); if req.task == "Forward": file_name = file_root+"/forward"; elif req.task == "Drop": file_name = file_root+"/drop"; else : return taskResponse(False); f = open(file_name,"r"); plan_file = RobotTrajectory(); buf = f.read(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); arm_right_group = moveit_commander.MoveGroupCommander("arm_right"); arm_left_group = moveit_commander.MoveGroupCommander("arm_left"); arm_left_group.set_start_state_to_current_state(); StartPnt = JointTrajectoryPoint(); StartPnt.positions = arm_left_group.get_current_joint_values(); StartPnt.velocities = [0]*len(StartPnt.positions); StartPnt. accelerations = [0]*len(StartPnt.positions); #print StartPnt; plan.joint_trajectory.points[0] = StartPnt; #print plan; display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); robot = moveit_commander.RobotCommander(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." arm_left_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); return taskResponse(True);
def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm="iterative_time_parameterization"): ser_ref_state_in = conversions.msg_to_string(ref_state_in) ser_traj_in = conversions.msg_to_string(traj_in) ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm) traj_out = RobotTrajectory() traj_out.deserialize(ser_traj_out) return traj_out
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ (ser_path, fraction) = self._g.compute_cartesian_path( [conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions) path = RobotTrajectory() path.deserialize(ser_path) return (path, fraction)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions=True, path_constraints=None): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ if path_constraints: if type(path_constraints) is Constraints: constraints_str = conversions.msg_to_string(path_constraints) else: raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints)) (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions, constraints_str) else: (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions) path = RobotTrajectory() path.deserialize(ser_path) return (path, fraction)
def runTrajectory(req): global Traj_server print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join(os.path.dirname(__file__), "../trajectories/Torso/bin" + req.bin_num) else: file_root = os.path.join(os.path.dirname(__file__), "../trajectories/bin" + req.bin_num) if req.task == "Forward": file_name = file_root + "/forward" elif req.task == "Drop": file_name = file_root + "/drop" elif req.task == "Pick": file_name = file_root + "/Pick" elif req.task == "Scan": file_name = file_root + "/scan" elif req.task == "Dump": file_name = file_root + "/Dump" elif req.task == "Lift": file_name = file_root + "/Lift" elif req.task == "Home": file_name = file_root + "/Home" elif req.task == "Rotate": file_name = file_root + "/Rotate" else: return taskResponse(False) f = open(file_name, "r") plan_file = RobotTrajectory() buf = f.read() plan_file.deserialize(buf) plan = copy.deepcopy(plan_file) f.close() return GetTrajectoryResponse(plan, True)
def plan(self, joints=None): """Return a tuple of the motion planning results such as (success flag : boolean, trajectory message : RobotTrajectory, planning time : float, error code : MoveitErrorCodes)""" if type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) elif joints is not None: try: self.set_joint_value_target( self.get_remembered_joint_values()[joints]) except MoveItCommanderException: self.set_joint_value_target(joints) (error_code_msg, trajectory_msg, planning_time) = self._g.plan() error_code = MoveItErrorCodes() error_code.deserialize(error_code_msg) plan = RobotTrajectory() return ( error_code.val == MoveItErrorCodes.SUCCESS, plan.deserialize(trajectory_msg), planning_time, error_code, )
def plan(self, joints = None): """ Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """ if type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) elif not joints == None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except: self.set_joint_value_target(joints) plan = RobotTrajectory() plan.deserialize(self._g.compute_plan()) return plan
def plan(self, joints=None): """ Return a tuple of the motion planning results such as (success flag : boolean, trajectory message : RobotTrajectory, planning time : float, error code : MoveitErrorCodes) """ if type(joints) is JointState: self.set_joint_value_target(joints) elif type(joints) is Pose: self.set_pose_target(joints) elif joints is not None: try: self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except MoveItCommanderException: self.set_joint_value_target(joints) (error_code_msg, trajectory_msg, planning_time) = self._g.plan() error_code = MoveItErrorCodes() error_code.deserialize(error_code_msg) plan = RobotTrajectory() return (error_code.val == MoveItErrorCodes.SUCCESS, plan.deserialize(trajectory_msg), planning_time, error_code)
def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, avoid_collisions = True): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, avoid_collisions) path = RobotTrajectory() path.deserialize(ser_path) return (path, fraction)
def runTrajectory(req): global Traj_server print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" if req.using_torso == "y": file_root = os.path.join( os.path.dirname(__file__), "../../optimize_trajectories/Torso/bin" + req.bin_num) else: file_root = os.path.join( os.path.dirname(__file__), "../../optimize_trajectories/bin" + req.bin_num) if req.task == "Forward": file_name = file_root + "/forward" elif req.task == "Drop": file_name = file_root + "/drop" else: return taskResponse(False) f = open(file_name, "r") plan_file = RobotTrajectory() buf = f.read() plan_file.deserialize(buf) plan = copy.deepcopy(plan_file) arm_right_group = moveit_commander.MoveGroupCommander("arm_right") arm_left_group = moveit_commander.MoveGroupCommander("arm_left") arm_left_group.set_start_state_to_current_state() StartPnt = JointTrajectoryPoint() StartPnt.positions = arm_left_group.get_current_joint_values() StartPnt.velocities = [0] * len(StartPnt.positions) StartPnt.accelerations = [0] * len(StartPnt.positions) #print StartPnt; plan.joint_trajectory.points[0] = StartPnt #print plan; display_trajectory_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory() robot = moveit_commander.RobotCommander() display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory) print "============ Waiting while", file_name, " is visualized (again)..." arm_left_group.execute(plan) print "Trajectory ", file_name, " finished!" f.close() return taskResponse(True)
" if type(joints) is JointState : self.set_joint_value_target(joints) elif type(joints) is Pose : self.set_pose_target(joints) elif joints is not None : try : self.set_joint_value_target(self.get_remembered_joint_values()[joints]) except MoveItCommanderException: self.set_joint_value_target(joints) (error_code_msg, trajectory_msg, planning_time) = self._g.plan() error_code = MoveItErrorCodes() error_code.deserialize(error_code_msg) plan = RobotTrajectory() return (error_code.val == MoveItErrorCodes.SUCCESS, plan.deserialize(trajectory_msg), planning_time, error_code) def compute_cartesian_path(self, waypoints, eef_step, jump_threshold, algorithm, avoid_collisions=True, path_constraints=None): """ Compute a sequence of waypoints that make the end-effector move in straight line segments that follow the poses specified as waypoints. Configurations are computed for every eef_step meters; The jump_threshold specifies the maximum distance in configuration space between consecutive points in the resultingpath; Kinematic constraints for the path given by path_constraints will be met for every point along the trajectory, if they are not met, a partial solution will be returned. The return value is a tuple: a fraction of how much of the path was followed, the actual RobotTrajectory. """ if path_constraints: if type(path_constraints) is Constraints: constraints_str = conversions.msg_to_string(path_constraints) else: raise MoveItCommanderException("Unable to set path constraints, unknown constraint type " + type(path_constraints)) (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, algorithm, avoid_collisions, constraints_str) else: (ser_path, fraction) = self._g.compute_cartesian_path([conversions.pose_to_list(p) for p in waypoints], eef_step, jump_threshold, algorithm, avoid_collisions) path = RobotTrajectory()
def runTrajectory(req): print "---------------------------------" print req.task print " " print req.bin_num print " " print req.using_torso print "---------------------------------" # Get the trajectory file_root = os.path.join(os.path.dirname(__file__), "../trajectories"); #file_root = os.path.join(os.path.dirname(__file__), "../traj_updates"); traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); plan = RobotTrajectory(); # Gripper Hand if req.task == "use_tray": file_name = file_root + "/left_arm_drop2home"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "use_gripper": file_name = file_root + "/left_arm_home2drop"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Forward": file_name = file_root+"/bin" + req.bin_num +"/forward"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Drop": file_name = file_root+"/bin" + req.bin_num+"/drop"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); elif req.task == "Scan": file_name = file_root+"/bin" + req.bin_num+"/scan"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_left_torso"); # Tray Hand elif req.task == "Pick": file_name = file_root+"/bin" + req.bin_num+"/Pick"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Dump": file_name = file_root+"/bin" + req.bin_num+"/Dump"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Lift": file_name = file_root+"/bin" + req.bin_num+"/Lift"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Home": file_name = file_root+"/bin" + req.bin_num+"/Home"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); elif req.task == "Rotate": file_name = file_root+"/bin" + req.bin_num+"/Rotate"; traj_execute_group = moveit_commander.MoveGroupCommander("arm_right_torso"); else : print "No plan selected, exit!"; return GetTrajectoryResponse(plan,True); f = open(file_name,"r"); buf = f.read(); plan_file = RobotTrajectory(); plan_file.deserialize(buf); plan = copy.deepcopy(plan_file); move_to_start_pos(plan, traj_execute_group); if check_trajectory_validity(plan): print "Trajectory is valid"; # Display Current Trajectory robot = moveit_commander.RobotCommander(); display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory) display_trajectory = moveit_msgs.msg.DisplayTrajectory(); display_trajectory.trajectory_start = robot.get_current_state() display_trajectory.trajectory.append(plan) display_trajectory_publisher.publish(display_trajectory); print "============ Waiting while", file_name, " is visualized (again)..." pose = PoseStamped() pose.header.frame_id = "/arm_left_link_7_t" pose.header.stamp = rospy.Time.now() pose.pose.position.x = 0 pose.pose.position.y = 0 pose.pose.position.z = -0.35 pose.pose.orientation.x = 0 pose.pose.orientation.y = 0 pose.pose.orientation.z = 0 pose.pose.orientation.w = 1 #attach_sphere("arm_left_link_7_t", "Object", pose, 0.17, ["hand_left_finger_1_link_2", "hand_left_finger_1_link_3", "hand_left_finger_1_link_3_tip", "hand_left_finger_2_link_2", "hand_left_finger_2_link_3", "hand_left_finger_2_link_3_tip", "hand_left_finger_middle_link_2", "hand_left_finger_middle_link_3", "hand_left_finger_middle_link_3_tip"]); traj_execute_group.execute(plan); print "Trajectory ", file_name, " finished!" f.close(); remove_object(); return GetTrajectoryResponse(plan,True); else: print "Trajectory is not valid! Test failed!"; return GetTrajectoryResponse(plan,True);