diff --git a/grinding_motion_routines/src/grinding_motion_routines/moveit_executor.py b/grinding_motion_routines/src/grinding_motion_routines/moveit_executor.py index ac1ac94..4f93d8e 100755 --- a/grinding_motion_routines/src/grinding_motion_routines/moveit_executor.py +++ b/grinding_motion_routines/src/grinding_motion_routines/moveit_executor.py @@ -16,7 +16,13 @@ class MoveitExecutor(object): """Executor including MoveItCommander . This class can command moving manipurator with single pose or way points.""" - def __init__(self, move_group_name, ee_link,planner_id="RRTConnectkConfigDefault",planning_time =20): + def __init__( + self, + move_group_name, + ee_link, + planner_id="RRTConnectkConfigDefault", + planning_time=20, + ): # initialize moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() @@ -36,23 +42,28 @@ def __init__(self, move_group_name, ee_link,planner_id="RRTConnectkConfigDefault queue_size=20, ) - # set planner move_group.set_planner_id(planner_id) - move_group.set_planning_time (planning_time) + move_group.set_planning_time(planning_time) # set end effector link move_group.set_end_effector_link(ee_link) # rospy.loginfo robot parameters - rospy.loginfo("============ Planning frame: %s" % move_group.get_planning_frame()) + rospy.loginfo( + "============ Planning frame: %s" % move_group.get_planning_frame() + ) rospy.loginfo("============ Planner ID: %s" % move_group.get_planner_id()) rospy.loginfo("============ Planning time: %s" % move_group.get_planning_time()) - rospy.loginfo("============ End effector link: %s" % move_group.get_end_effector_link()) - rospy.loginfo("============ End effector pose: %s" % move_group.get_current_pose()) + rospy.loginfo( + "============ End effector link: %s" % move_group.get_end_effector_link() + ) + rospy.loginfo( + "============ End effector pose: %s" % move_group.get_current_pose() + ) rospy.loginfo( "============ Available Planning Groups: %s" % robot.get_group_names() ) - + # Robot state rospy.loginfo("============ Rospy.loginfoing robot state") rospy.loginfo(robot.get_current_state()) @@ -97,6 +108,7 @@ def _list_to_pose(self, pose_list): pose_msg.orientation.w = pose_list[6] return pose_msg + def change_planner_id(self, planner_id): self.move_group.set_planner_id(planner_id) rospy.loginfo("============ Planner ID: %s" % planner_id) @@ -145,7 +157,6 @@ def execute_cartesian_path_to_goal_pose( acc_scale=0.1, execute=True, eef_step=0.01, - jump_threshold=0.0, avoid_collisions=True, path_constraints=None, number_of_waypoints=10, @@ -175,7 +186,6 @@ def execute_cartesian_path_to_goal_pose( acc_scale=acc_scale, execute=execute, eef_step=eef_step, - jump_threshold=jump_threshold, avoid_collisions=avoid_collisions, path_constraints=path_constraints, ) @@ -189,7 +199,6 @@ def execute_cartesian_path_by_waypoints( acc_scale=0.1, execute=True, eef_step=0.01, - jump_threshold=0.0, avoid_collisions=True, path_constraints=None, ): @@ -205,7 +214,6 @@ def execute_cartesian_path_by_waypoints( (path, fraction) = self.move_group.compute_cartesian_path( waypoints, eef_step=eef_step, - jump_threshold=jump_threshold, avoid_collisions=avoid_collisions, path_constraints=path_constraints, )