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 joint_states_publisher Node:**
The Sensor data from the Arduino is published with Rosserial. This topic is subscribed to by my joint_states_publisher 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 **follow_joint_trajectory 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 ros_control loading a joint_state_publisher and an FollowJointTrajectoryAction controller from a controllers.yaml config file and the trajectory is executed smoothly. This lead me to try using ros_control 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](https://github.com/PickNikRobotics/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?
↧