Open jacknlliu opened 6 years ago
It does it exactly the same way. My only guess is that you restart gazebo time which break the ros_control
plugin.
It can't provide the exact joint data, even though we never reset gazebo time.
I found that the joint data from the gazebo API directly seems reasonable, see here, but after this function angles::shortest_angular_distance()
, the joint angles become very large after robot had a collision with some object in gazebo.
I think I should provide a basic ros package for proving this.
That would be great. If we can reproduce it cleanly, we can fix it.
Hi, @bmagyar Sorry for the delay. Could you try the kinova_ros package with the following command?
roslaunch kinova_gazebo robot_launch.launch kinova_robotType:=j2n6s300
After the robot initial, we can find that joint-state-controller
provides the wrong joint positions.
The issue can be easily reproduced using the ur_robot_driver repository. It starts after certain collision circumstances. In order to reproduce it, in any ros workspace:
git clone https://github.com/UniversalRobots/Universal_Robots_ROS_Driver.git src/Universal_Robots_ROS_Driver
git clone -b calibration_devel https://github.com/fmauch/universal_robot.git src/fmauch_universal_robot
sudo apt update -qq
rosdep update
rosdep install --from-paths src --ignore-src -y
catkin_make
or
catkin build
source devel/setup.bash
roslaunch ur_gazebo ur3e_bringup.launch
rostopic echo /joint_states
rostopic pub /pos_joint_traj_controller/command trajectory_msgs/JointTrajectory "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
joint_names:
- "shoulder_pan_joint"
- "shoulder_lift_joint"
- "elbow_joint"
- "wrist_1_joint"
- "wrist_2_joint"
- "wrist_3_joint"
points:
- positions: [0,3.14,0,0,0,0]
velocities: [0,0,0,0,0,0]
accelerations: [0,0,0,0,0,0]
effort: [0,0,0,0,0,0]
time_from_start: {secs: 4, nsecs: 0}" -1
The robot should try to go to that position crossing the floor and while heavy colliding with the floor the bug appears, showing a joint position that exceed by far the actual limits.
After some further experimentation, you can check how they're actually the correct joint values but outside the bounds. For example, when sending that command for the first time, I got the following positions values: [18.849573233427748, 3.1399866768081672, 18.849762255449107, 3.7085518315604915e-05, -43.98228751908111, -6.283167197213825] As you can see those are equivalent to: [0, 3.1399866768081672, 0, 0, 0, 0] But outside the 2pi range. Now if I command the first joint to move to 4 rad I get: [22.849555914307878, 3.1399866768081672, 18.849762255449107, 3.7085518315604915e-05, -43.98228751908111, -6.283167197213825] That is also equivalent to: [4, 3.1399866768081672, 0, 0, 0, 0]
This issue reminds me of https://github.com/ros/angles/issues/2 , that was actually closed long ago but with those weird collision states may be related.
The issue has been detected and reported in the gazebo_ros_pkgs repository as the main bug concerns the gazebo_ros_control package sources. https://github.com/ros-simulation/gazebo_ros_pkgs/issues/1405#issue-1292401140
When using
joint_state_controller
with gazebo,/joint_states
will provide wrong and large joint data when robot has heavy collision with some object in the gazebo world. But if we use gazebo pluginjoint_state_publisher
, the joint states will be right!I find that gazebo plugin
joint_state_publisher
gets the joint states directly from gazebo, see here. But I can't get where the joint states update injoint_sate_controller
.How does the
joint_state_[i]
update in the following codes? https://github.com/ros-controls/ros_controllers/blob/e5a6c5a5f9f84756b6601961a5b03b7dffc5118c/joint_state_controller/src/joint_state_controller.cpp#L93