fetch_gazebo should configure cartesian twist controller

[rongoldman-tri]

Hi. Is there any documentation for enabling & using the cartesian_twist_controller in gazebo to control the Fetch's manipulator?

I tried to add it to the fetch_gazebo/config/default_controllers.yaml file:

cartesian_twist_controller:
type: "robot_controllers/CartesianTwistController"
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- upperarm_roll_joint
- elbow_flex_joint
- forearm_roll_joint
- wrist_flex_joint
- wrist_roll_joint

and added it to the list of the gazebo/default_controllers.

If I run the start_controller.py script:

% ./start_controller.py cartesian_twist_controller
[INFO] [WallTime: 1502395440.391452] [0.000000] Connecting to /query_controller_states...
[INFO] [WallTime: 1502395440.721294] [147.571000] Done.
[INFO] [WallTime: 1502395440.721661] [147.571000] Requesting that cartesian_twist_controller be started...
[INFO] [WallTime: 1502395440.725579] [147.572000] Done.
%

but in the shell where gazebo was launched I see:

[ INFO] [1502395440.722918140, 147.571000000]: Stopped arm_controller/follow_joint_trajectory
[ INFO] [1502395440.723480279, 147.571000000]: Started cartesian_twist_controller
[ INFO] [1502395440.726726733, 147.572000000]: Stopped cartesian_twist_controller

In Gazebo the simulated arm then collapses, presumably because gravity compensation has been disabled. Running the get_controller_state.py script shows the twist controller as stopped:

% ./get_controller_state.py
[INFO] [WallTime: 1502395379.218454] [0.000000] Connecting to /query_controller_states...
[INFO] [WallTime: 1502395379.585420] [104.226000] Requesting state of controllers...
arm_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: RUNNING
arm_controller/gravity_compensation[robot_controllers/GravityCompensation]: RUNNING
arm_with_torso_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: STOPPED
base_controller[robot_controllers/DiffDriveBaseController]: RUNNING
head_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: RUNNING
head_controller/point_head[robot_controllers/PointHeadController]: STOPPED
torso_controller/follow_joint_trajectory[robot_controllers/FollowJointTrajectoryController]: STOPPED
gripper_controller/gripper_action[robot_controllers/ParallelGripperController]: RUNNING
bellows_controller[robot_controllers/ScaledMimicController]: RUNNING
cartesian_pose_controller[robot_controllers/CartesianPoseController]: STOPPED
cartesian_twist_controller[robot_controllers/CartesianTwistController]: STOPPED

So what do I need to do to make this work?

Also will the same procedure enable the cartesian_twist_controller on a Fetch robot?

thanks,

-- Ron --

[mikeferguson]

(Also answering on mailing list)

We've recently added support on the robot for the twist controller (primarily for joystick control of the end effector). The public ROS farm just sync'd over the weekend, and we should have an update to our packages later this week once everything has passed qualification. The commit that added the twist controller to the physical robot is:

ZebraDevs/fetch_robots@06bb495

As you'll see, the controller takes a chain, rather than a list of joints. I would also make sure that you have robot_controllers 0.5.3 -- there were some bug fixes as this was the first time the twist controller had been used on a robot.

I'm not 100% sure why the arm would collapse. I can't actually say that I've tested this part in Gazebo, which is a bit of an oversight, I've updating this ticket to track fixing that disparity with the real robot.

[connormbrooks]

For those interested, I was able to enable the controller in Gazebo simply by adding the lines from the commit referenced above (ZebraDevs/fetch_robots@06bb495) to fetch_gazebo/config/default_controllers.yaml !