Panda robot causing BoxedLcpConstraintSolver when loading using urdf with ign-ros2-control

09ubberboy90 commented 2 years ago

Environment

OS Version: Ubuntu 20.04
Source or binary build? Using source branch foxy with commit d1f8bec223bb98e77ff0042992d6c9bffc206942

Description

Expected behavior: Robot stays upright when simulation starts and no error in console
Actual behavior: Ignition launches with the robot present and as soon as the simulation is started using the play button it collapses like the second picture and following error is spammed in the console

[ign gazebo-1] Error [BoxedLcpConstraintSolver.cpp:291] [BoxedLcpConstraintSolver] The solution of LCP includes NAN values: nan nan. We're setting it zero for safety. Consider using more robust solver such as PGS as a secondary solver. If this happens even with PGS solver, please report this as a bug.

Steps to reproduce

Install ign_ros2_control and moveit_resources_panda_description
run export IGN_GAZEBO_RESOURCE_PATH=/opt/ros/foxy/share/
run attached launch file after modifying path to urdf
Press play in ignition panda_gripper.zip

Output

Log included in the zip file

If the following is removed then no physics error happen but the robot doesn't have any joint control.

 <gazebo>
    <plugin filename="ignition_ros2_control-system" name="ignition_ros2_control::IgnitionROS2ControlPlugin">
      <robot_param>robot_description</robot_param>
      <robot_param_node>robot_state_publisher</robot_param_node>
      <parameters>/workspaces/Ignition/ubb/ignitiondev/install/share/simple_arm/config/panda_ros_controllers.yaml</parameters>
    </plugin>
  </gazebo>

AndrejOrsula commented 2 years ago

Hello @09ubberboy90,

I am unsure what is causing this issue but if it's of any help, you can find a functional setup and examples for Panda under panda_ign_moveit2/ign_moveit2_examples. It was only tested on rolling/fortress though :confused:.

09ubberboy90 commented 2 years ago

Hi Thanks for that. Using this and your answer from Gazebo forum I was able to modify my config to get my robot working. However I did not use your version of ros_control as it had some compilling problem with ros foxy. I instead used this fork from SC-Robotics.

I did use your PID though :)

Not sure if I should close since It's more of a workaround then an actual fix since I'm not using my urdf but your panda asset from ignition fuel

ros-controls / gz_ros2_control