MoveIt Trajectory not Executed Correctly on Real Robot
Hello,
I got a robot arm using an Arduino Mega 2560. It uses 5 Servo motors to actuate the joints and I can read the current joint angle from 5 Sensors as well. In my Arduino Code I am currently able to receive Joy messages to actuate the motors forward or backwards, as well as declaring a goal joint position for each motor to be run to. This robot I want to connect to MoveIt.
It is still not clear to me, what is the proper way to do so, even though I tried for a few weeks now. Currently my problem is that the path I receive is not the one that is shown as planned in Rviz.
What I currently did:
1. Create a jointstatespublisher Node:
The Sensor data from the Arduino is published with Rosserial. This topic is subscribed to by my jointstatespublisher Node, which publishes the message for all angles on the /joint_states topic, as well as publishing the /tf transform.
2. Setup MoveIt:
Created the MoveIt package with the setup assistant manager. Created the controllers.yaml file to let MoveIt be a client in a followjointtrajectory Action. The current robot state is displayed in Rviz and I can start planning for Goal Positions.
controller_list:
- name: boris_control/arm_action_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- base_rotation_joint
- upper_arm_joint
- elbow_joint
- lower_arm_joint
- wrist_joint
- hand_joint
- finger_joint
3. Created an Action Server Node:
From what I understand I need to implement an Action server that receives the plan from MoveIt, sends it to the Arduino and monitors it's execution.
The Action Server (ActionLib) contains a ExecuteCB method that gets called when a FollowJointTrajectoryGoal message is sent from MoveIt. I am currently sending each way point of the plan at a time, wait for it to be executed and continue with the next point. I am ignoring the velocity, effort and time values sent by MoveIt.
However, the problem is that the planned path does not match the plan I receive. The starting and goal positions are correct and if I only send those, the execution works. But when I want to reach all joint positions from the plan to drive around an object for example, the joint positions in between are not those shown in the plan by Rviz. I get the plan from the goal message in my Action Server like this:
void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
//...
for (int i = 0; i < goal -> trajectory.points.size(); i++) {
// Set next joint angles as goal to be reached.
moveit_command.angle1 = goal -> trajectory.points[i].positions[0];
// ...
publisher.publish(moveit_command); // Looping till this is reached...
}
I logged those positions and it turns out they are not the ones the plan is showing. Here it can be seen that the plan is to move the arm all the way up, but on execution it drives through the collision object only lifting the arm half way.
4. What I tried:
I can connect MoveIt with Gazebo using roscontrol loading a jointstatepublisher and an FollowJointTrajectoryAction controller from a controllers.yaml config file and the trajectory is executed smoothly. This lead me to try using roscontrol instead of ActionLib. However, getting into that seems a lot more complicated.
I tried to implement a Hardware Interface using the BoilerPlate found here: ros control boilerplate
I setup the controllers and connected it with my Arduino. I do not quite understand how to implement the write() and read() methods for the controllers, though. What I did was pretty much the same I did in my Action Server before. Subscribe to the /joint_states in read, and publish the joint position command in write. But this results in the same outcome I had before.
This leads me into thinking that just publishing the desired joint angles is not the correct approach to this? But what else do I have to do?
Could anyone tell me what I do wrong and what would be the correct way to do this?
Edit:
Update1:
Here is a picture that shows what is wrong. I logged the values I receive in my Action Server and checked the 31st positions as an example.
void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
for (int i = 0; i < goal -> trajectory.points.size(); i++) {
// cmd.pos1 = goal -> trajectory.points[i].positions[x]; ...
ROS_INFO("Waypoint[%d / %d]", i, (int)goal -> trajectory.points.size() - 1);
ROS_WARN("%s \t: %.2f", goal -> trajectory.joint_names[0].c_str(), cmd.pos1);
ROS_WARN("%s \t: %.2f", goal -> trajectory.joint_names[5].c_str(), cmd.pos2);
ROS_WARN("%s \t: %.2f", goal -> trajectory.joint_names[4].c_str(), cmd.pos3);
ROS_WARN("%s \t\t: %.2f", goal -> trajectory.joint_names[3].c_str(), cmd.pos4);
ROS_WARN("%s \t\t: %.2f", goal -> trajectory.joint_names[2].c_str(), cmd.pos5);
}
}
The first picture shows the planned path values displayed in MoveIt (at waypoint 31) and the log during execution. The second shows those values executed (set in Rviz with the joint state publisher gui).
The link going through the robot can be ignored since it is not considered for collision checking. As it can be seen, the joint positions do not match the displayed planned positions. Why is that? Only the starting and goal joint positions are correct.
As I said before, I am ignoring velocities, accelerations and timefromstart values sent by MoveIt, because my plan to just set the joint positions and wait for each one to be reached before going to the next. Is this approach wrong? Do I have to calculate the positions somehow with all values received?
Edit:
Update2:
When using the ros control boilerplate to create a controller for my robot, the "simulation" that comes with it works. Here the joint states is just set at start and overwritten with the new command from what I can tell. The result can be seen here: sim_control
void SimHWInterface::read(ros::Duration &elapsed_time) {
// No need to read since our write() command populates our state for us
}
void SimHWInterface::write(ros::Duration &elapsed_time) {
// Safety
enforceLimits(elapsed_time);
for (std::size_t joint_id = 0; joint_id < num_joints_; ++joint_id) {
joint_position_[joint_id] = joint_position_command_[joint_id];
}
}
The control loop pretty much only consists of the read, update and write methods. From what i understand:
In read the robot's current joint states have to be retrieved for the controller to know where the robot is at.
In update ros_control calculates the command needed to execute the trajectory.
In write the command has to be sent to the robot for it to be executed.
So I changed the code as follows, adding a publisher and subscriber, but the result is the same like when i used the basic ActionLib Action server. The result can be seen here: real_control
void SimHWInterface::read(ros::Duration &elapsed_time) {
// Fill current_joint_position with recieved data.
joint_position_[0] = cur_state.angle1;
joint_position_[1] = cur_state.angle2;
joint_position_[2] = cur_state.angle2; // Mimic joint
joint_position_[3] = cur_state.angle3;
joint_position_[4] = cur_state.angle3; // Mimic joint
joint_position_[5] = cur_state.angle4;
joint_position_[6] = cur_state.angle5;
}
void SimHWInterface::write(ros::Duration &elapsed_time) {
// Safety
enforceLimits(elapsed_time);
boris_ros::BorisJointState move_cmd;
move_cmd.angle1 = joint_position_command_[0];
move_cmd.angle2 = joint_position_command_[1];
move_cmd.angle3 = joint_position_command_[3];
move_cmd.angle4 = joint_position_command_[5];
move_cmd.angle5 = joint_position_command_[6];
publisher.publish(move_cmd);
}
Asked by JulianR on 2018-09-08 11:06:40 UTC
Comments
ROS uses radians for everything (REP-103). Do your servo motors use degrees by any chance? Without converting between the two weird things will happen ..
Asked by gvdhoorn on 2018-09-08 13:22:15 UTC
Btw; please consider a topic title change. The current title suggests that you don't understand how to "connect" MoveIt to "a real robot", but that is not the case. The steps you describe are exactly what needs to be done. You're actual problem appears to be your robot doing ..
Asked by gvdhoorn on 2018-09-08 13:24:25 UTC
.. something that you don't expect. That is something else and it would be good if your topic title reflected that.
Another comment: I see that your robot consists of a closed-loop kinematics structure. That is not supported by URDF, nor MoveIt, so depending on how you modelled things, that may ..
Asked by gvdhoorn on 2018-09-08 13:24:44 UTC
.. also cause the issues you are having.
Asked by gvdhoorn on 2018-09-08 15:00:53 UTC
Thank you for confirming that I am in the correct way.
My sensor data is in degree,, but I covert them into radian. (degree / 180 * M_PI)
Asked by JulianR on 2018-09-08 23:01:25 UTC
Does closed loop kinematic structure mean those two connections between links? If so I tried to mimic those in the urdf, but because moveit does not support mimic joints I ended up "mimicing" those in my joint state publisher, for planning these are still weird which is why I disable collision...
Asked by JulianR on 2018-09-08 23:03:07 UTC
Checking between those links and the robot. After execution they are at the correct position though.
Asked by JulianR on 2018-09-08 23:03:26 UTC
and the other way around? For sending commands to your servos?
Do you also convert from radians to degrees?
Asked by gvdhoorn on 2018-09-09 04:01:49 UTC
Yes I also convert the values back.
I did some more testing and posted a picture below. There it can be seen that the positions I receive in my callback are not the ones I should get. Maybe my approach of just taking the positions and ignoring velocities, accelerations and time_from_start is wrong?
Asked by JulianR on 2018-09-09 05:37:49 UTC
I believe we're going to have to take a look at your urdf. Right now I have no idea what is happening.
I'm also not sure what "it's not the values I should be receiving" means. Are you saying that the action goal doesn't contain the same trajectory as MoveIt plans?
Asked by gvdhoorn on 2018-09-09 11:16:06 UTC
That's what it looks like to me.
I log the action goal position values I receive and set them manually in Rviz, but those positions do not match the plan that is shown when MoveIt plans for that goal. It seems like the execution is correct, but the plan I receive is not.
Asked by JulianR on 2018-09-09 14:28:42 UTC
Or are the positions I receive from the action goal not the joint angles for all way points for that trajectory?
The urdf is exported from Solidworks.
urdf
Asked by JulianR on 2018-09-09 14:31:12 UTC
I would recommend to work on your URDF first. The joints make no sense to me, and all those
rpy
attributes being set to!= 0
is not healthy. If with "exported from SolidWorks" you mean you used the SolidWorks to URDF plugin, then that would explain it. Create a minimal kinematic ..Asked by gvdhoorn on 2018-09-09 14:34:24 UTC
.. model of your robot (forget all the parallel joints), then try again.
You also have no proper collision geometry. Using (highly) detailed models for collision geometry is going to slow things down .. a lot.
The simpler your urdf is, the better things will work.
Asked by gvdhoorn on 2018-09-09 14:36:03 UTC
I'm not saying you're wrong, but I've worked with this infrastructure for quite some time, and this would be the first time I'd encounter something like this. I suspect your URDF before MoveIt at this point.
Asked by gvdhoorn on 2018-09-09 14:38:51 UTC
Here is a small GIF that shows exactly what is happening. GIF
Yes I used the SolidWorks to URDF plugin. I will try to work on my model and see what I can do. What about the joints makes no sense to you?
Thank you for your efforts so far
Asked by JulianR on 2018-09-09 14:58:16 UTC
Are you absolutely sure your conversions are ok? Try using the angles for it.
The motion shown in the gif looks like a scaled down version of the trajectory, that's why I ask.
re: urdf: try to verify your setup with a simpler URDF. It may shed some light on things.
Asked by gvdhoorn on 2018-09-09 15:15:53 UTC
I used angles and the result was the same.
My first thought was about a wrong conversion too. But what I do not understand is why the first and last trajectory points would be correct but the others are not.
I will try to work on my urdf next and see what I can do there.
Asked by JulianR on 2018-09-10 04:08:44 UTC
Something I just assumed, but let's check: are your servos able to complete the motion in time? Example: if your servos need 5 seconds to complete a trajectory segment, but you only give them 2 seconds, then that would result in about half the trajectory segment being executed.
Asked by gvdhoorn on 2018-09-10 04:15:23 UTC
I edited my post to show what happens when I try to use ros_control. What I do not understand is why the "simulation" where joint states are just set work, but when it is executed on my robot the positions seem different.
Asked by JulianR on 2018-09-10 05:05:15 UTC
I did not consider execution time until now, because my plan was to wait in my Action Server till a sent position is reached and only after that position was reached send the next one. Maybe that is the wrong approach?
Asked by JulianR on 2018-09-10 05:07:25 UTC
I found what is wrong.
Elbow and Wrist Joints should be Mimic joints, but MoveIt does not support those as far as I know.
I always expected the mimic joints to have the same value as the their "parent", but MoveIt ofc plans for them as normal joints, which is not what I need.
Asked by JulianR on 2018-09-11 08:00:29 UTC
Is there any way to use MoveIt with mimic joints within a move group?
Or is there a way to calculate the needed joint position from combining both values, of the parent and the mimic joint? I am currently out of ideas how to continue.
Asked by JulianR on 2018-09-11 08:02:47 UTC