Problems to use MoveIt! with the physical OpenManipulator-PRO

[KaikeWesleyReis]

Hello! I'm having problems to use this arm with MoveIt for sequential position. The task that I'm trying to perform is:

Move to poseA ​and then move to poseB​, sequentially

The Python sample code bellow is an example:

When I try this code in a Gazebo simuation works correctly. But, when I try with the physical arm it is try to do both positions at the same time (not sequentially). Actually, I'm solving this problem using a rospy.sleep(10) between poses, but I want to know what is happen!

System Information

• ROS Melodic
• Ubuntu 18.04
• MoveIt! 1.0 with Python

[JaehyunShim]

@KaikeWesleyReis

Unfortunately, we cannot help you with your own code.

Ryan

[KaikeWesleyReis]

@rjshim I understand, but my point is that the problem is not in the code itself. As I not mentioned, this sample code is based on MoveIt! tutorial and works for others two robots in my lab (UR5 and Jaco from Kinova) correctly and not for OpenManipulator-PRO. My point is that the problem could be in the MoveIt! code from the OpenManipulator-PRO package. I'm just asking if it is possible to do this test, because apparently the nature Action-Server from MoveIt! is not working correctly.

[JaehyunShim]

@KaikeWesleyReis

Because the response from the controller is not 'goal reached' but 'path planned', you are having that issue. I will give you a tip but I can't guarantee it will work. Try below and let me know the result. 1) use function 'go_to_pose' for the first goal pose 2) while ('states' topic message is moving) {print("moving");} 3) use function 'go_to_pose' for the next goal pose.

Ryan

[KaikeWesleyReis]

@rjshim I just came to the lab today. I did not understand your second step, could you explain more?

[JaehyunShim]

@KaikeWesleyReis Echo the 'states' topic while om-pro controller node is running, then you will know.

[JaehyunShim]

As there has been no update, I will close this issue.

Ryan

[KaikeWesleyReis]

Hello @rjshim sorry for the delay, I was on vacation. I tried what you said, but the results was weird. I made: rostopic echo / /open_manipulator_pro/states then I used a sample code to move my robot to some pose. The results in echo was something like this:

---
open_manipulator_moving_state: "\"IS_MOVING\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---
open_manipulator_moving_state: "\"STOPPED\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---
---
open_manipulator_moving_state: "\"IS_MOVING\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---
open_manipulator_moving_state: "\"STOPPED\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---
(...)
---
open_manipulator_moving_state: "\"STOPPED\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---
---
open_manipulator_moving_state: "\"STOPPED\""
open_manipulator_actuator_state: "\"ACTUATOR_ENABLED\""
---

Let's me explain: at movement beginning IS_MOVING and STOPPED alternates, during the movement (...) echo only 'STOPPED', however the arm was in movement.

[JaehyunShim]

@KaikeWesleyReis

This is a sample code of what you want to implement but it is written in ROS2. Function names are all the same so you will get a brief idea of how you should make yours.

Ryan

[KaikeWesleyReis]

@rjshim

I'm glad that you are helping, but my C++ skills are not high level as yours. I'm trying to understand your code, but I was not able to get my solution from there. I will make some assumptions, and you can say if I'm correct or not:

• 1 - For your code, you are not using any MoveIt! but a service to set your joints.
• 2 - The main functions that I need to focus are: OpenManipulatorXDemo::set_joint_space_path OpenManipulatorXDemo::process_callback The first one, set your joints target and move your robot and the second only manage different positions to move sequentially based on state_ value.
• 3 - In set_joint_space_path, you control your robot based in time, represented by path_time and not based on target/joint_state comparison (what I'm saying that you are not using a process like return if actual_joint_state == target_position_state). So it is correct to that if a movement time exceeds path_time, the robot will stop to move and go for other position (based on state_)
• 4 - How does exactly works this piece of code:

  auto response_received_callback = [this](ServiceResponseFuture future) 
  {
  auto result = future.get();
  return result->is_planned;
  };
  auto future_result = goal_tool_control_client_->async_send_request(request, response_received_callback);

• 5 - async_send_request is an action server?

Again, thanks for your help and I think if you clarify those points I can get a better understanding.

[KaikeWesleyReis]

@rjshim any answer?

[JaehyunShim]

@KaikeWesleyReis

I am sorry that I can't debug your code or explain more about ROS. Let us know if there are any problems with our examples.

Ryan

[KaikeWesleyReis]

@rjshim Well, as I said the problem was in OpenManipulator ROBOTIS implemented code. But OK, you can close this.