|
@@ -1,18 +1,18 @@
|
|
|
|
|
|
-from logging.handlers import RotatingFileHandler
|
|
|
import os
|
|
|
import moveit_msgs.msg
|
|
|
import rospy, sys
|
|
|
import moveit_commander
|
|
|
import moveit_msgs
|
|
|
+import tf.transformations
|
|
|
|
|
|
+from logging.handlers import RotatingFileHandler
|
|
|
from moveit_commander import MoveGroupCommander, PlanningSceneInterface, RobotCommander
|
|
|
from moveit_msgs.msg import PlanningScene, ObjectColor,CollisionObject, AttachedCollisionObject,RobotTrajectory
|
|
|
from moveit_msgs.msg import RobotState
|
|
|
from moveit_msgs.msg import Constraints, PositionConstraint, OrientationConstraint
|
|
|
from shape_msgs.msg import SolidPrimitive
|
|
|
from sensor_msgs.msg import JointState
|
|
|
-import tf.transformations
|
|
|
from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint
|
|
|
from geometry_msgs.msg import PoseStamped, Pose,Point,Vector3
|
|
|
|
|
@@ -32,9 +32,9 @@ pi = np.pi
|
|
|
class MoveIt_Control:
|
|
|
|
|
|
def __init__(self, is_use_gripper=False):
|
|
|
- self.home_pose=[]
|
|
|
- self.hf_num=0
|
|
|
- self.hf_fail=[]
|
|
|
+ self.home_pose = []
|
|
|
+ self.hf_num = 0
|
|
|
+ self.hf_fail = []
|
|
|
|
|
|
moveit_commander.roscpp_initialize(sys.argv)
|
|
|
self.robot = moveit_commander.RobotCommander()
|
|
@@ -61,12 +61,12 @@ class MoveIt_Control:
|
|
|
|
|
|
|
|
|
self.go_home()
|
|
|
- self.home_pose=self.get_now_pose()
|
|
|
+ self.home_pose = self.get_now_pose()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
- def plan_cartesian_path_LLL(self,point_s,point_e,waypoints,trajectory,trajectory_with_type,v=1):
|
|
|
+ def plan_cartesian_path_LLL(self, point_s, point_e, waypoints, trajectory, trajectory_with_type, v=1):
|
|
|
fraction = 0.0
|
|
|
maxtries = 50
|
|
|
attempts = 0
|
|
@@ -74,36 +74,36 @@ class MoveIt_Control:
|
|
|
|
|
|
start_pose = self.arm.get_current_pose(self.end_effector_link).pose
|
|
|
wpose = deepcopy(start_pose)
|
|
|
- waypoint=[]
|
|
|
+ waypoint = []
|
|
|
|
|
|
if waypoints:
|
|
|
- wpose.position.x=waypoints[-1].position.x
|
|
|
- wpose.position.y=waypoints[-1].position.y
|
|
|
- wpose.position.z=waypoints[-1].position.z
|
|
|
- wpose.orientation.x=waypoints[-1].orientation.x
|
|
|
- wpose.orientation.y=waypoints[-1].orientation.y
|
|
|
- wpose.orientation.z=waypoints[-1].orientation.z
|
|
|
- wpose.orientation.w=waypoints[-1].orientation.w
|
|
|
+ wpose.position.x = waypoints[-1].position.x
|
|
|
+ wpose.position.y = waypoints[-1].position.y
|
|
|
+ wpose.position.z = waypoints[-1].position.z
|
|
|
+ wpose.orientation.x = waypoints[-1].orientation.x
|
|
|
+ wpose.orientation.y = waypoints[-1].orientation.y
|
|
|
+ wpose.orientation.z = waypoints[-1].orientation.z
|
|
|
+ wpose.orientation.w = waypoints[-1].orientation.w
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
|
|
|
|
|
|
- wpose.position.x=point_s[0]
|
|
|
- wpose.position.y=point_s[1]
|
|
|
- wpose.position.z=point_s[2]
|
|
|
- wpose.orientation.x=point_s[3]
|
|
|
- wpose.orientation.y=point_s[4]
|
|
|
- wpose.orientation.z=point_s[5]
|
|
|
- wpose.orientation.w=point_s[6]
|
|
|
+ wpose.position.x = point_s[0]
|
|
|
+ wpose.position.y = point_s[1]
|
|
|
+ wpose.position.z = point_s[2]
|
|
|
+ wpose.orientation.x = point_s[3]
|
|
|
+ wpose.orientation.y = point_s[4]
|
|
|
+ wpose.orientation.z = point_s[5]
|
|
|
+ wpose.orientation.w = point_s[6]
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
waypoints.append(deepcopy(wpose))
|
|
|
|
|
|
- wpose.position.x=point_e[0]
|
|
|
- wpose.position.y=point_e[1]
|
|
|
- wpose.position.z=point_e[2]
|
|
|
- wpose.orientation.x=point_e[3]
|
|
|
- wpose.orientation.y=point_e[4]
|
|
|
- wpose.orientation.z=point_e[5]
|
|
|
- wpose.orientation.w=point_e[6]
|
|
|
+ wpose.position.x = point_e[0]
|
|
|
+ wpose.position.y = point_e[1]
|
|
|
+ wpose.position.z = point_e[2]
|
|
|
+ wpose.orientation.x = point_e[3]
|
|
|
+ wpose.orientation.y = point_e[4]
|
|
|
+ wpose.orientation.z = point_e[5]
|
|
|
+ wpose.orientation.w = point_e[6]
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
waypoints.append(deepcopy(wpose))
|
|
|
|
|
@@ -120,8 +120,8 @@ class MoveIt_Control:
|
|
|
rospy.loginfo("Path computed successfully.")
|
|
|
|
|
|
trajj = plan
|
|
|
- retimed_planed=self.retime_plan(trajj,v)
|
|
|
- trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
|
|
|
+ retimed_planed = self.retime_plan(trajj, v)
|
|
|
+ trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
|
|
|
trajj_with_type.points[-len(trajj.joint_trajectory.points)].type = "start"
|
|
|
trajj_with_type.points[-1].type = "end"
|
|
|
trajectory.append(trajj)
|
|
@@ -129,20 +129,15 @@ class MoveIt_Control:
|
|
|
moveit_server.arm.execute(retimed_planed)
|
|
|
rospy.loginfo("执行结束")
|
|
|
trajectory_with_type.append(trajj_with_type)
|
|
|
- err=0
|
|
|
+ err = 0
|
|
|
else:
|
|
|
- err=1
|
|
|
+ err = 1
|
|
|
rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))
|
|
|
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- return waypoints,trajectory,trajectory_with_type,err
|
|
|
+ return waypoints, trajectory, trajectory_with_type, err
|
|
|
+
|
|
|
|
|
|
- def plan_cartesian_path_point(self,point_ss,point_ee,waypoints,trajectory,trajectory_with_type,v=1):
|
|
|
+ def plan_cartesian_path_point(self, point_ss, point_ee, waypoints, trajectory, trajectory_with_type, v=1):
|
|
|
fraction = 0.0
|
|
|
maxtries = 50
|
|
|
attempts = 0
|
|
@@ -150,36 +145,36 @@ class MoveIt_Control:
|
|
|
|
|
|
start_pose = self.arm.get_current_pose(self.end_effector_link).pose
|
|
|
wpose = deepcopy(start_pose)
|
|
|
- waypoint=[]
|
|
|
+ waypoint = []
|
|
|
|
|
|
if waypoints:
|
|
|
- wpose.position.x=waypoints[-1].position.x
|
|
|
- wpose.position.y=waypoints[-1].position.y
|
|
|
- wpose.position.z=waypoints[-1].position.z
|
|
|
- wpose.orientation.x=waypoints[-1].orientation.x
|
|
|
- wpose.orientation.y=waypoints[-1].orientation.y
|
|
|
- wpose.orientation.z=waypoints[-1].orientation.z
|
|
|
- wpose.orientation.w=waypoints[-1].orientation.w
|
|
|
+ wpose.position.x = waypoints[-1].position.x
|
|
|
+ wpose.position.y = waypoints[-1].position.y
|
|
|
+ wpose.position.z = waypoints[-1].position.z
|
|
|
+ wpose.orientation.x = waypoints[-1].orientation.x
|
|
|
+ wpose.orientation.y = waypoints[-1].orientation.y
|
|
|
+ wpose.orientation.z = waypoints[-1].orientation.z
|
|
|
+ wpose.orientation.w = waypoints[-1].orientation.w
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
|
|
|
|
|
|
- wpose.position.x=point_ss[0]
|
|
|
- wpose.position.y=point_ss[1]
|
|
|
- wpose.position.z=point_ss[2]
|
|
|
- wpose.orientation.x=point_ss[3]
|
|
|
- wpose.orientation.y=point_ss[4]
|
|
|
- wpose.orientation.z=point_ss[5]
|
|
|
- wpose.orientation.w=point_ss[6]
|
|
|
+ wpose.position.x = point_ss[0]
|
|
|
+ wpose.position.y = point_ss[1]
|
|
|
+ wpose.position.z = point_ss[2]
|
|
|
+ wpose.orientation.x = point_ss[3]
|
|
|
+ wpose.orientation.y = point_ss[4]
|
|
|
+ wpose.orientation.z = point_ss[5]
|
|
|
+ wpose.orientation.w = point_ss[6]
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
waypoints.append(deepcopy(wpose))
|
|
|
|
|
|
- wpose.position.x=point_ee[0]
|
|
|
- wpose.position.y=point_ee[1]
|
|
|
- wpose.position.z=point_ee[2]
|
|
|
- wpose.orientation.x=point_ee[3]
|
|
|
- wpose.orientation.y=point_ee[4]
|
|
|
- wpose.orientation.z=point_ee[5]
|
|
|
- wpose.orientation.w=point_ee[6]
|
|
|
+ wpose.position.x = point_ee[0]
|
|
|
+ wpose.position.y = point_ee[1]
|
|
|
+ wpose.position.z = point_ee[2]
|
|
|
+ wpose.orientation.x = point_ee[3]
|
|
|
+ wpose.orientation.y = point_ee[4]
|
|
|
+ wpose.orientation.z = point_ee[5]
|
|
|
+ wpose.orientation.w = point_ee[6]
|
|
|
waypoint.append(deepcopy(wpose))
|
|
|
waypoints.append(deepcopy(wpose))
|
|
|
|
|
@@ -196,30 +191,23 @@ class MoveIt_Control:
|
|
|
rospy.loginfo("Path computed successfully.")
|
|
|
|
|
|
trajj = plan
|
|
|
- retimed_planed=self.retime_plan(trajj,v)
|
|
|
- trajj_with_type = mark_the_traj(trajj,"during-l",welding_sequence)
|
|
|
-
|
|
|
-
|
|
|
+ retimed_planed = self.retime_plan(trajj, v)
|
|
|
+ trajj_with_type = mark_the_traj(trajj, "during-l", welding_sequence)
|
|
|
trajectory.append(trajj)
|
|
|
rospy.loginfo("开始执行")
|
|
|
moveit_server.arm.execute(retimed_planed)
|
|
|
rospy.loginfo("执行结束")
|
|
|
trajectory_with_type.append(trajj_with_type)
|
|
|
- err=0
|
|
|
+ err = 0
|
|
|
else:
|
|
|
- err=1
|
|
|
+ err = 1
|
|
|
rospy.loginfo("fraction=" + str(fraction) + " failed after " + str(maxtries))
|
|
|
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- return waypoints,trajectory,trajectory_with_type,err
|
|
|
+ return waypoints, trajectory, trajectory_with_type, err
|
|
|
+
|
|
|
|
|
|
|
|
|
- def retime_plan(self,traj,v):
|
|
|
+ def retime_plan(self, traj, v):
|
|
|
current_state = self.arm.get_current_state()
|
|
|
|
|
|
retimed_traj = self.arm.retime_trajectory(
|
|
@@ -230,10 +218,11 @@ class MoveIt_Control:
|
|
|
|
|
|
return retimed_traj
|
|
|
|
|
|
+
|
|
|
def get_now_pose(self):
|
|
|
|
|
|
current_pose = self.arm.get_current_pose(self.end_effector_link).pose
|
|
|
- pose=[]
|
|
|
+ pose = []
|
|
|
pose.append(current_pose.position.x)
|
|
|
pose.append(current_pose.position.y)
|
|
|
pose.append(current_pose.position.z)
|
|
@@ -245,130 +234,20 @@ class MoveIt_Control:
|
|
|
|
|
|
|
|
|
return pose
|
|
|
-
|
|
|
- def zhu_shi():
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
- pass
|
|
|
|
|
|
|
|
|
- def go_home(self,a=1,v=1):
|
|
|
+ def go_home(self, a=1, v=1):
|
|
|
rospy.loginfo("go_home start")
|
|
|
self.arm.set_max_acceleration_scaling_factor(a)
|
|
|
self.arm.set_max_velocity_scaling_factor(v)
|
|
|
self.arm.set_named_target('home')
|
|
|
- rospy.loginfo("get_home end")
|
|
|
+ rospy.loginfo("get home_point")
|
|
|
self.arm.go()
|
|
|
rospy.loginfo("go_home end")
|
|
|
|
|
|
|
|
|
-
|
|
|
|
|
|
- def go_home_justplan(self,trajectory,trajectory_with_type,a=1,v=1):
|
|
|
+ def go_home_justplan(self, trajectory, trajectory_with_type, a=1, v=1):
|
|
|
rospy.loginfo("go_home start")
|
|
|
self.arm.set_max_acceleration_scaling_factor(a)
|
|
|
self.arm.set_max_velocity_scaling_factor(v)
|
|
@@ -381,36 +260,36 @@ class MoveIt_Control:
|
|
|
trajj = traj[1]
|
|
|
moveit_server.arm.execute(trajj)
|
|
|
rospy.loginfo("go_home end")
|
|
|
- traj_with_type = mark_the_traj(trajj,"go-home",welding_sequence)
|
|
|
+ traj_with_type = mark_the_traj(trajj, "go-home", welding_sequence)
|
|
|
trajectory.append(trajj)
|
|
|
trajectory_with_type.append(traj_with_type)
|
|
|
return trajectory,trajectory_with_type
|
|
|
|
|
|
-
|
|
|
|
|
|
- def path_planning(self,folder_path,gohome=True):
|
|
|
+
|
|
|
+ def path_planning(self, folder_path, gohome=True):
|
|
|
file_path_result = os.path.join(folder_path, 'result.txt')
|
|
|
all_data = process_welding_data(file_path_result)
|
|
|
|
|
|
- way_points,trajectory,trajectory_with_type = [],[],[]
|
|
|
+ way_points,trajectory,trajectory_with_type = [], [], []
|
|
|
for i in range(len(all_data)):
|
|
|
- rospy.loginfo("共读取到%d条焊缝,开始规划第%d条",len(all_data),i+1)
|
|
|
+ rospy.loginfo("共读取到%d条焊缝,开始规划第%d条", len(all_data), i+1)
|
|
|
start_point = all_data[i][0]
|
|
|
end_point = all_data[i][1]
|
|
|
q1 = all_data[i][2]
|
|
|
q2 = all_data[i][3]
|
|
|
|
|
|
- point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0],q1[1],q1[2],q1[3]]
|
|
|
- point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0],q2[1],q2[2],q2[3]]
|
|
|
+ point11 = [start_point[0]/1000, start_point[1]/1000, start_point[2]/1000, q1[0], q1[1], q1[2], q1[3]]
|
|
|
+ point22 = [end_point[0]/1000, end_point[1]/1000, end_point[2]/1000, q2[0], q2[1], q2[2], q2[3]]
|
|
|
|
|
|
- up_value=-0.1
|
|
|
- point_up=[point11[0], point11[1], point11[2]-up_value, point11[3], point11[4],point11[5],point11[6]]
|
|
|
+ up_value = -0.1
|
|
|
+ point_up = [point11[0], point11[1], point11[2]-up_value, point11[3], point11[4], point11[5], point11[6]]
|
|
|
|
|
|
- way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_point(point_up,point11,way_points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
+ way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_point(point_up, point11, way_points, trajectory, trajectory_with_type, v=speed_v)
|
|
|
rospy.loginfo("末端执行器上移完毕")
|
|
|
rospy.loginfo("*******************")
|
|
|
|
|
|
- if(error==1):
|
|
|
+ if(error == 1):
|
|
|
rospy.loginfo("焊缝规划出现失败 清空路径列表")
|
|
|
rospy.loginfo("*******************")
|
|
|
trajectory.clear()
|
|
@@ -418,11 +297,11 @@ class MoveIt_Control:
|
|
|
break
|
|
|
|
|
|
|
|
|
- way_points,trajectory,trajectory_with_type,error = self.plan_cartesian_path_LLL(point11,point22,way_points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
+ way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_LLL(point11, point22, way_points, trajectory, trajectory_with_type, v=speed_v)
|
|
|
|
|
|
- rospy.loginfo("第%d条焊缝规划完毕",i+1)
|
|
|
+ rospy.loginfo("第%d条焊缝规划完毕", i+1)
|
|
|
rospy.loginfo("*******************")
|
|
|
- if(error==1):
|
|
|
+ if(error == 1):
|
|
|
rospy.loginfo("焊缝规划出现失败 清空路径列表")
|
|
|
rospy.loginfo("*******************")
|
|
|
trajectory.clear()
|
|
@@ -430,14 +309,14 @@ class MoveIt_Control:
|
|
|
break
|
|
|
|
|
|
if i<len(all_data):
|
|
|
- up_value=-0.1
|
|
|
- point_up=[point22[0], point22[1], point22[2]-up_value, point22[3], point22[4],point22[5],point22[6]]
|
|
|
+ up_value = -0.1
|
|
|
+ point_up = [point22[0], point22[1], point22[2]-up_value, point22[3], point22[4], point22[5], point22[6]]
|
|
|
|
|
|
- way_points, trajectory, trajectory_with_type,error = self.plan_cartesian_path_point(point22,point_up,way_points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
+ way_points, trajectory, trajectory_with_type, error = self.plan_cartesian_path_point(point22, point_up, way_points,trajectory,trajectory_with_type,v=speed_v)
|
|
|
rospy.loginfo("末端执行器上移完毕")
|
|
|
rospy.loginfo("*******************")
|
|
|
|
|
|
- if(error==1):
|
|
|
+ if(error == 1):
|
|
|
rospy.loginfo("焊缝规划出现失败 清空路径列表")
|
|
|
rospy.loginfo("*******************")
|
|
|
trajectory.clear()
|
|
@@ -447,11 +326,11 @@ class MoveIt_Control:
|
|
|
|
|
|
if gohome:
|
|
|
|
|
|
- trajectory,trajectory_with_type = self.go_home_justplan(trajectory,trajectory_with_type)
|
|
|
+ trajectory, trajectory_with_type = self.go_home_justplan(trajectory, trajectory_with_type)
|
|
|
|
|
|
- traj_merge= merge_trajectories(trajectory)
|
|
|
- trajectory_with_type_merge= merge_trajectories_with_type(trajectory_with_type)
|
|
|
- return trajectory,traj_merge,trajectory_with_type_merge
|
|
|
+ traj_merge = merge_trajectories(trajectory)
|
|
|
+ trajectory_with_type_merge = merge_trajectories_with_type(trajectory_with_type)
|
|
|
+ return trajectory, traj_merge, trajectory_with_type_merge
|
|
|
|
|
|
|
|
|
def process_welding_data(filename):
|
|
@@ -478,16 +357,17 @@ def mark_the_traj(traj,TYPE,SEQUENCE):
|
|
|
traj_with_type.joint_names = traj.joint_trajectory.joint_names
|
|
|
traj_with_type.points = [
|
|
|
JointTrajectoryPoint_ex(
|
|
|
- positions=point.positions,
|
|
|
- velocities=point.velocities,
|
|
|
- accelerations=point.accelerations,
|
|
|
- effort=point.effort,
|
|
|
- type=TYPE,
|
|
|
- sequence=SEQUENCE
|
|
|
+ positions = point.positions,
|
|
|
+ velocities = point.velocities,
|
|
|
+ accelerations = point.accelerations,
|
|
|
+ effort = point.effort,
|
|
|
+ type = TYPE,
|
|
|
+ sequence = SEQUENCE
|
|
|
) for point in traj.joint_trajectory.points
|
|
|
]
|
|
|
return traj_with_type
|
|
|
|
|
|
+
|
|
|
def merge_trajectories(trajectories):
|
|
|
if not trajectories:
|
|
|
return None
|
|
@@ -547,11 +427,12 @@ def merge_trajectories_with_type(trajectory_with_type):
|
|
|
|
|
|
|
|
|
class py_msgs():
|
|
|
- fial=[]
|
|
|
- shun_xv=[]
|
|
|
+ fial = []
|
|
|
+ shun_xv = []
|
|
|
class point():
|
|
|
- xyz_list=[]
|
|
|
- type=[]
|
|
|
+ xyz_list = []
|
|
|
+ type = []
|
|
|
+
|
|
|
|
|
|
|
|
|
def ROS2PY_msgs(msgs, m_sr):
|
|
@@ -626,11 +507,11 @@ if __name__ =="__main__":
|
|
|
moveit_server = MoveIt_Control(is_use_gripper=False)
|
|
|
welding_sequence = rospy.get_param('welding_sequence')
|
|
|
|
|
|
- speed_v=0.001
|
|
|
+ speed_v = 0.001
|
|
|
|
|
|
|
|
|
|
|
|
- trajectory,trajectory_merge,trajectory_with_type_merge = moveit_server.path_planning(folder_path)
|
|
|
+ trajectory, trajectory_merge, trajectory_with_type_merge = moveit_server.path_planning(folder_path)
|
|
|
|
|
|
|
|
|
|