panda flops to ground when trying ex_ign_control.launch.py

[parkbytes]

Hi @AndrejOrsula ,

I am trying out the new ign_moveit2_examples based on ign_ros2_control and have it working partially. The follow example works to a large extent but the throw example simply moves the arm without picking the object. In the latter case, I noticed the panda arm flops to ground initially and then starts the swing action from there without moving to grip the object first. To isolate the problem, I thought I'll try the example launch file included with panda_moveit_config independently and observed the panda arm flops here as well. Panda in gazebo does respond to plan and execute actions from Rviz though and the fake_controller examples works fine too.

I am not sure if the behaviour I am observing is to be expected. The controller nodes are running and active and I thought this should hold the arm in position based on the robot description. Appreciate any pointers for troubleshooting.

My environment is ROS 2 galactic with Ignition Edifice and MoveIt 2 (binary installs). Rest of dependencies are built from source as per your instructions.

Launch example used:

ros2 launch panda_moveit_config ex_ign_control.launch.py

ros2 node list

/controller_manager
/gripper_trajectory_controller
/ignition_ros_control
/interactive_marker_display_94654427760256
/joint_state_broadcaster
/joint_trajectory_controller
/move_group
/move_group_interface_node
/move_group_private_93902434562128
/moveit_simple_controller_manager
/robot_state_publisher
/ros_ign_bridge
/rviz
/rviz_private_139885273182464
/servo_node
/transform_listener_impl_55675d4475b0
/transform_listener_impl_561672ea10b0
/transform_listener_impl_5616737ce9c0
/transform_listener_impl_7f3994049890

ros2 control list_controllers

joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active    
joint_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active    
gripper_trajectory_controller[joint_trajectory_controller/JointTrajectoryController] active

Observed the below warning/error snippets in log. Not sure if these are related.

[servo_node_main-4] [WARN] [1641621321.469220564] [moveit_servo.servo_node]: Intra-process communication is disabled, consider enabling it by adding: 
[servo_node_main-4] extra_arguments=[{'use_intra_process_comms' : True}]
[servo_node_main-4] to the Servo composable node in the launch file

[move_group-3] [WARN] [1641621321.544445816] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead

[ign gazebo-1] Warning [GenericJoint.hpp:1480] [GenericJoint::setRestPosition] Value of _q0 [0], is out of the limit range [-3.07178, -0.0698132] for index [0] of Joint [Joint].
[ign gazebo-1] [INFO] [1641621323.299285190] [controller_manager]: Loading controller 'joint_trajectory_controller'
[ign gazebo-1] [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v115ModelE], which doesn't have `operator<<`. Component will not be serialized.
[ign gazebo-1] [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v115WorldE], which doesn't have `operator<<`. Component will not be serialized.
[ign gazebo-1] [WARN] [1641621323.311442987] [ignition_ros_control]:  Desired controller update period (0.004 s) is slower than the gazebo simulation period (0.001 s).

[rviz2-5] [ERROR] [1641621325.382359723] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available

[AndrejOrsula]

Hello @parkbytes,

I noticed the panda arm flops to ground initially and then starts the swing action from there without moving to grip the object first.

"Flopping" of the robot at the beginning of simulation occurs when using ign_ros2_control with effort-only command interface. Once a command is sent, it is no longer a problem. Therefore, I suspect that the controller doesn't take the initial joint configuration into account when it is initialized with effort-only command interface. Sending initial command fixes it, so I have not yet investigated this issue further. Since there is no easy way to insert robot with a specific joint configuration (https://github.com/AndrejOrsula/panda_ign_moveit2/issues/10), one must initialize the robot somehow anyway. You can also try changing the command interface to position/velocity/position,velocity (multiple places) and run scripts/xacro2sdf.bash before the examples, however, the robot wouldn't be able to interact with object and simply clip through them.

I am unsure why the first motion of the example is skipped for you, but it has to do something with timing if the rest of the motion work. Below are some suggestions to debug it:

• Add a short sleep in the example node before starting the motion.
• Make sure that all nodes have their use_sim_time set to true. For ign_ros2_control, I have only tried hard-coding it so far in https://github.com/AndrejOrsula/ign_ros2_control/commit/6d3bc70f5ddc72b4bbb6ce6b06390c6849c264c0.
• Run the example individually after the environment is fully setup (the rest of the launch script)

To conclude, I believe that the flopping of the robot is an upstream issue but it is easy to find a workaround - which is necessary anyway since the robot is initialized with 0 joint positions. Skipping of the first motion is a timing issue, using use_sim_time everywhere should prevent it.

---

Nodes seem fine and the errors shouldn't cause such problems.

The following WARN is just a performance thing where servo is not launched in a composable way (you can try to change it if you want in move_group.launch.py).

[servo_node_main-4] [WARN] [1641621321.469220564] [moveit_servo.servo_node]: Intra-process communication is disabled, consider enabling it by adding: 
[servo_node_main-4] extra_arguments=[{'use_intra_process_comms' : True}]
[servo_node_main-4] to the Servo composable node in the launch file

The following WARN is irrelevant as octomap is not being used.

[move_group-3] [WARN] [1641621321.544445816] [moveit.ros.occupancy_map_monitor.middleware_handle]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead

This one is connected with not being able to initialize the joints with a custom configuration.

[ign gazebo-1] Warning [GenericJoint.hpp:1480] [GenericJoint::setRestPosition] Value of _q0 [0], is out of the limit range [-3.07178, -0.0698132] for index [0] of Joint [Joint].

You can play with the simulation step_size and controller frequency to make these match if you want.

[ign gazebo-1] [WARN] [1641621323.311442987] [ignition_ros_control]:  Desired controller update period (0.004 s) is slower than the gazebo simulation period (0.001 s).

Not sure what this one is referring to, but it's probably fine.

[ign gazebo-1] [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v115ModelE], which doesn't have `operator<<`. Component will not be serialized.
[ign gazebo-1] [Wrn] [Component.hh:144] Trying to serialize component with data type [N3sdf3v115WorldE], which doesn't have `operator<<`. Component will not be serialized.

Just ignore this error. no issue there either.

[rviz2-5] [ERROR] [1641621325.382359723] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available

[parkbytes]

Hello @AndrejOrsula, Thank you for the response above. Sorry for the delay in reverting as I have been learning along with trying out your suggestions.

1. Changed the command interface to 'position' and observed the robot no longer collapses but, as you stated, it clips through the object when trying the throw example.

2. Reverted back the command interface to 'effort' and tried introducing a short sleep in the example before starting motion. As before, the robot moves into some odd joint configuration and doesn't achieve the throw motion as expected.

3. I modified the example to send the command to achieve initial joint configuration as a first step (i.e. before moving to pose goal above the object) and this made a difference. The robot is able to move to the desired pose goals and grasp the object and start the the throw motion. Faced a new problem here, however, as the object slips off the gripper after the initial lift. Tried playing with the gripper friction values but didn't help. Finally, i reduced the mass of the throwing box object to 0.1 from 0.6 and the robot is able to pick and execute the throw action trajectory. But the object release is not as expected and it falls short of the target. The swing in the throw action appears very slow and looks like the robot is not able to achieve the desired velocity at release, though max_velocity and max_acceleration are set to 1 in code.

I am continuing my investigation but appreciate any suggestions on fixing the issue with object slipping off the gripper (with the original mass) and diagnosing the joint efforts/velocity during throw. Thanks

[AndrejOrsula]

Hey,

Regarding the object slipping over, you could also try to play with the force that the gripper applies. Currently, panda is configured to use JointTrajectoryController - therefore, try playing with PID gains: https://github.com/AndrejOrsula/panda_ign_moveit2/blob/369b0ab7d205bfcbadcd5d9a7ad6d886c9dca51f/panda_moveit_config/config/controllers_effort.yaml#L77-L86 You could also try to configure the gripper to use GripperCommand if you want.

Not sure about the object falling without a proper throw. Probably a physics issue, so you could try reducing the step_size. Also, it is just a toy example after all - so don't expect it to work flawlessly every time.

[parkbytes]

Thank you. I guess I'll close this issue here and keep experimenting. I persisted with your example, despite knowing it's not built for robust performance, as it helps with my learning! :)