0
$\begingroup$

quite new to ROS. I'm trying to program the trajectory for a 3DoF manipulator using MoveIt! I have been able to visualize it in RViz using MoveIt setup assistant and made sure to setup planning groups, for the manipulator and for the hand [end effector] as well as pre-defined poses and also defined the parent link for the end effector.

When running demo.launch from my moveit package I'm able to move the joints with the sliders and simulate the planning. Problem comes when I try to set up a pre-defined pose using a python script. From demo.launch terminal I get the following:

[ INFO] [1711704513.046623852]: Execution completed: SUCCEEDED
[ INFO] [1711704515.147534827]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1711704515.150170180]: Planner configuration 'manipulator' will use planner 'geometric::RRT'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1711704515.150431874]: manipulator/manipulator: Starting planning with 1 states already in datastructure
[ INFO] [1711704515.159576743]: manipulator/manipulator: Created 798 states
[ INFO] [1711704515.159591030]: Solution found in 0.009201 seconds
[ INFO] [1711704515.159612830]: SimpleSetup: Path simplification took 0.000000 seconds and changed from 2 to 2 states
[ INFO] [1711704516.162209723]: Execution request received
[ INFO] [1711704516.246796290]: Fake execution of trajectory
[ INFO] [1711704516.646727431]: Completed trajectory execution with status SUCCEEDED ...
[ INFO] [1711704516.646941847]: Execution completed: SUCCEEDED

Read it might have something to do with the Fake execution of trajectory, but don't really know how to change that.

And in my python script terminal:

[INFO] [1711704487.846684]: Now at Pose: zero_pose
[INFO] [1711704489.848485]: Going to Pose: pose1
[INFO] [1711704491.448173]: Now at Pose: pose1
[INFO] [1711704493.452515]: Going to Pose: zero_pose
[INFO] [1711704495.046906]: Now at Pose: zero_pose
[INFO] [1711704497.048577]: Going to Pose: pose1

Seems that my manipulator is reaching the desired pose correctly, but I'm not able to visualize it in my RViz screen, it stands at zero_pose. Something that I noted is that it stands at zero_pose but the manipulator is transparent, it looks the same way to when I used the sliders for individual joint movement.

I added TF in RViz and links are reaching the expected position. But no visualization of it.

Here's my python script:

class MyRobot:

    def __init__(self, Group_Name):

        self._commander = moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('node_set_redefined_pose', anonymous=True)

        self._robot = moveit_commander.RobotCommander()
        self._scene = moveit_commander.PlanningSceneInterface()
        
        self._planning_group = Group_Name

        self._group = moveit_commander.MoveGroupCommander(self._planning_group)
        
        self._display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)

        self._exectute_trajectory_client = actionlib.SimpleActionClient('execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
        self._exectute_trajectory_client.wait_for_server()

        self._planning_frame = self._group.get_planning_frame()
        self._eef_link = self._group.get_end_effector_link()
        self._group_names = self._robot.get_group_names()

      
        rospy.loginfo('\033[95m' + "Planning Group: {}".format(self._planning_frame) + '\033[0m')
        rospy.loginfo('\033[95m' + "End Effector Link: {}".format(self._eef_link) + '\033[0m')
        rospy.loginfo('\033[95m' + "Group Names: {}".format(self._group_names) + '\033[0m')
        rospy.loginfo('\033[95m' + " >>> MyRobot initialization is done." + '\033[0m')

    def set_pose(self, arg_pose_name):
        rospy.loginfo('\033[32m' + "Going to Pose: {}".format(arg_pose_name) + '\033[0m')
        
        success, plan, _, _ = self._group.plan()  

        if not success:
            rospy.logerr("No plan found for pose: {}".format(arg_pose_name))
            return
        
        display_trajectory = moveit_msgs.msg.DisplayTrajectory()
        display_trajectory.trajectory_start = self._robot.get_current_state()
        display_trajectory.trajectory.append(plan)
        self._display_trajectory_publisher.publish(display_trajectory)
        rospy.sleep(1)  

        goal = moveit_msgs.msg.ExecuteTrajectoryGoal()

        goal.trajectory = plan 

        self._exectute_trajectory_client.send_goal(goal)
        self._exectute_trajectory_client.wait_for_result()

        rospy.loginfo('\033[32m' + "Now at Pose: {}".format(arg_pose_name) + '\033[0m')

    def __del__(self):
        moveit_commander.roscpp_shutdown()
        rospy.loginfo(
            '\033[95m' + "Object of class MyRobot Deleted." + '\033[0m')

I echo /move_group/display_planned_path and it's being published correctly

$\endgroup$

0

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.