Model breaks when using JointTrajectoryController
Hello Developers!
I trying to use an Effort controller called `JointTrajectoryController' in order to have the robot arm pass through certain joint angle way-points. Although it runs with this controller for ~ 4 seconds (Simulation time), the Gazebo model breaks and the joint states report nan values for all the joints.
I will begin by sharing my Yaml file (borrowed the top half of the syntax from here), the launch file script that calls the point-to-point controller (`wamjointscontrol' in the Yaml file) and the trajectory follower controller ('effbasedpostrajcontroller' in the Yaml file), the control switcher in my python script, the function that sends the commands to the publisher in the form of batches of the trajectory, the publisher itself, and a snippet of the way-points itself.
For debugging, I am including the ROS Environment variables
Yaml file
# https://github.com/ros-industrial/ur_modern_driver/issues/305
ros_control_namespace: "/"
# MoveIt-specific simulation settings
# Settings for ros_control control loop
generic_hw_control_loop:
loop_hz: 300
cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
joints:
- uwarl_a_wam/base_yaw_joint
- uwarl_a_wam/shoulder_pitch_joint
- uwarl_a_wam/shoulder_yaw_joint
- uwarl_a_wam/elbow_pitch_joint
- uwarl_a_wam/wrist_yaw_joint
- uwarl_a_wam/wrist_pitch_joint
- uwarl_a_wam/palm_yaw_joint
sim_control_mode: 0 # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
controller_list:
- name: /eff_based_pos_traj_controller
action_ns: follow_joint_trajectory
default: True
type: FollowJointTrajectory
joints:
- uwarl_a_wam/base_yaw_joint
- uwarl_a_wam/shoulder_pitch_joint
- uwarl_a_wam/shoulder_yaw_joint
- uwarl_a_wam/elbow_pitch_joint
- uwarl_a_wam/wrist_yaw_joint
- uwarl_a_wam/wrist_pitch_joint
- uwarl_a_wam/palm_yaw_joint
- name: /wam_joints_control
action_ns: follow_joint_trajectory
default: True
type: std_msgs/Float64
joints:
- uwarl_a_wam/base_yaw_joint
- uwarl_a_wam/shoulder_pitch_joint
- uwarl_a_wam/shoulder_yaw_joint
- uwarl_a_wam/elbow_pitch_joint
- uwarl_a_wam/wrist_yaw_joint
- uwarl_a_wam/wrist_pitch_joint
- uwarl_a_wam/palm_yaw_joint
# Reference values
# https://git.barrett.com/software/libbarrett_ros/blob/ros_control/config/wam_config.yaml
eff_based_pos_traj_controller:
type: effort_controllers/JointTrajectoryController
joints:
- uwarl_a_wam/base_yaw_joint
- uwarl_a_wam/shoulder_pitch_joint
- uwarl_a_wam/shoulder_yaw_joint
- uwarl_a_wam/elbow_pitch_joint
- uwarl_a_wam/wrist_yaw_joint
- uwarl_a_wam/wrist_pitch_joint
- uwarl_a_wam/palm_yaw_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
uwarl_a_wam/base_yaw_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/shoulder_pitch_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/shoulder_yaw_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/elbow_pitch_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/wrist_yaw_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/wrist_pitch_joint: {trajectory: 0.1, goal: 0.1}
uwarl_a_wam/palm_yaw_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
action_monitor_rate: 10
gains:
uwarl_a_wam/base_yaw_joint:
p: 900
d: 10
i: 2.5
i_clamp: 0
uwarl_a_wam/shoulder_pitch_joint:
p: 2500
d: 20
i: 5
i_clamp: 0
uwarl_a_wam/shoulder_yaw_joint:
p: 600
d: 5
i: 2
i_clamp: 0
uwarl_a_wam/elbow_pitch_joint:
p: 500
d: 2
i: 0.5
i_clamp: 0
uwarl_a_wam/wrist_yaw_joint:
p: 50
d: 0.5
i: 0.5
i_clamp: 0
uwarl_a_wam/wrist_pitch_joint:
p: 50
d: 0.5
i: 0.5
i_clamp: 0
uwarl_a_wam/palm_yaw_joint:
p: 8
d: 0.05
i: 0.1
i_clamp: 0
wam_joints_control:
# Reference values
# https://git.barrett.com/software/libbarrett_ros/blob/ros_control/config/wam_config.yaml
# Publish all joint states -----------------------------------
# Position Controllers ---------------------------------------
joint1_position_controller:
joint: uwarl_a_wam/base_yaw_joint
type: effort_controllers/JointPositionController
pid: {p: 900.0, i: 2.5, d: 10.0}
joint2_position_controller:
joint: uwarl_a_wam/shoulder_pitch_joint
type: effort_controllers/JointPositionController
pid: {p: 2500.0, i: 5, d: 20.0}
joint3_position_controller:
joint: uwarl_a_wam/shoulder_yaw_joint
type: effort_controllers/JointPositionController
pid: {p: 600.0, i: 2, d: 5}
joint4_position_controller:
joint: uwarl_a_wam/elbow_pitch_joint
type: effort_controllers/JointPositionController
pid: {p: 500.0, i: 0.5, d: 2}
joint5_position_controller:
joint: uwarl_a_wam/wrist_yaw_joint
type: effort_controllers/JointPositionController
pid: {p: 50.0, i: 0.5, d: 0.5}
joint6_position_controller:
joint: uwarl_a_wam/wrist_pitch_joint
type: effort_controllers/JointPositionController
pid: {p: 50.0, i: 0.5, d: 0.5}
joint7_position_controller:
joint: uwarl_a_wam/palm_yaw_joint
type: effort_controllers/JointPositionController
pid: {p: 8.0, i: 0.1, d: 0.05}
Controller Launch file snippet
<group if="$(eval arg('hardware_interface')=='effort')">
<!-- Effort Based -->
<!-- WAM Trajectory Controller -->
<rosparam file="$(find waterloo_steel_control)/config/sim/wam_control_effort_V3.yaml" command="load"/>
<!-- WAM Position Controller-->
<node name="wam_position_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="wam_joints_control/joint1_position_controller
wam_joints_control/joint2_position_controller
wam_joints_control/joint3_position_controller
wam_joints_control/joint4_position_controller
wam_joints_control/joint5_position_controller
wam_joints_control/joint6_position_controller
wam_joints_control/joint7_position_controller
" />
<!-- WAM Trajectory Position Controller-->
<node name="wam_traj_position_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="--stopped
eff_based_pos_traj_controller
" />
<!-- BHand Controller-->
<rosparam file="$(find waterloo_steel_control)/config/sim/bhand_control_effort_V1.yaml" command="load"/>
<!-- BHand Effort Controller -->
<node name="bhand_position_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
args="bhand_joints_control/joint1
bhand_joints_control/joint2
bhand_joints_control/joint3
bhand_joints_control/joint4
bhand_joints_control/joint5
bhand_joints_control/joint6
bhand_joints_control/joint7
bhand_joints_control/joint8
" />
</group>
Control Switcher
def switch_controllers(self,activate_controller):
rospy.loginfo("Switching WAM Controller to "+activate_controller+" controller...")
if activate_controller == "pos_trajectory":
ret = self.switch_controller(['eff_based_pos_traj_controller'],
['wam_joints_control/joint1_position_controller',
'wam_joints_control/joint2_position_controller',
'wam_joints_control/joint3_position_controller',
'wam_joints_control/joint4_position_controller',
'wam_joints_control/joint5_position_controller',
'wam_joints_control/joint6_position_controller',
'wam_joints_control/joint7_position_controller'],
2,
True,
0)
print(ret)
rospy.loginfo("Switched WAM Controller to "+activate_controller+" controller...")
elif activate_controller == "pos_p2p":
ret = self.switch_controller(['wam_joints_control/joint1_position_controller',
'wam_joints_control/joint2_position_controller',
'wam_joints_control/joint3_position_controller',
'wam_joints_control/joint4_position_controller',
'wam_joints_control/joint5_position_controller',
'wam_joints_control/joint6_position_controller',
'wam_joints_control/joint7_position_controller'],
['eff_based_pos_traj_controller'],
2,
True,
0)
print(ret)
rospy.loginfo("Switched WAM Controller to "+activate_controller+" controller...")
else:
print("New controller. who dis?")
Switching Controller
# Switch Controllers
cart_pulling_object.switch_controllers("pos_trajectory")
rospy.sleep(duration=0.5)
# Start moving wagon
cart_pulling_object.cart_pulling_V2(time_step = 0.1)
Batch Trajectory sender function
def cart_pulling_V2(self,time_step):
# Read the joint motion trajectory file
with open('/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_viz/waterloo_steel_sim_bringup/script/gazebo_inputs.txt', 'r') as traj_file:
way_points = [[float(num) for num in line.split(',')] for line in traj_file]
rospy.loginfo("Starting WAM trajectory motion ...")
# Number of way-points
num_way_points = len(way_points)
# Construct an array of time-instances from going from the zeroth waypoint to the final one
time_steps = []
for i in range(num_way_points):
time_steps.append([i*time_step])
time_steps = np.asarray(time_steps)
# print(time_steps)
# print(time_steps.shape)
# Pass it to the trajectory generator
delta_steps = 10
for i in range(90):
start_index = 1+i*10
end_index = 1+i*10+delta_steps
# Submit a batch of way-points to pass through
self.summit_jointmover_object.move_wam_traj_all_joints_position(way_points[start_index:end_index],time_steps[1:,:])
# Pause for the required duration of time
rospy.sleep(duration=time_step*delta_steps)
rospy.loginfo(str(i+1)+" seconds of planned trajectory elapsed ...")
rospy.loginfo("Stopped base motion...")
Publisher
def move_wam_traj_all_joints_position(self, WAM_pos_traj,time_traj):
wam_joint_traj = JointTrajectory()
# Enter the joint names
# print(self.joint_states.name)
wam_joint_traj.joint_names = ['uwarl_a_wam/base_yaw_joint',
'uwarl_a_wam/shoulder_pitch_joint',
'uwarl_a_wam/shoulder_yaw_joint',
'uwarl_a_wam/elbow_pitch_joint',
'uwarl_a_wam/wrist_yaw_joint',
'uwarl_a_wam/wrist_pitch_joint',
'uwarl_a_wam/palm_yaw_joint']
# Enter the time at which we want the topic to be published
wam_joint_traj.header.stamp = rospy.Time.now()
# For each entry in the trajectory array, add it to the trajectory message
# https://www.programcreek.com/python/example/123228/trajectory_msgs.msg.JointTrajectory
for (position,time_step) in zip(WAM_pos_traj,time_traj):
# print(position[3:10])
# Initialize a joint trajectory point object to hold the way-point information
joint_traj_point = JointTrajectoryPoint()
# Add the way-point point (may alos add velocity at that point)
joint_traj_point.positions = position[3:9]
# Add time step-size for reaching this way-point from the previous way-point
joint_traj_point.time_from_start = rospy.Duration(time_step)
# Append this way-point information in the trajectory message
wam_joint_traj.points.append(joint_traj_point)
# Send sequence of trajectory way-points to trajectory publisher
self.pub_wam_joint_position_array.publish(wam_joint_traj)
Publisher declaration
#!/usr/bin/env python
# from asyncio import base_tasks
import rospy
import time
from math import pi, sin, cos, acos
import random
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
from geometry_msgs.msg import Twist
"""
Topics To Write on:
WAM traj
type: trajectory_msgs/JointTrajectory
/uwarl_a/eff_based_pos_traj_controller/command
WAM point-to-point
type: std_msgs/Float64
/uwarl_a/wam_joints_control/joint1/command
/uwarl_a/wam_joints_control/joint2/command
/uwarl_a/wam_joints_control/joint3/command
/uwarl_a/wam_joints_control/joint4/command
/uwarl_a/wam_joints_control/joint5/command
/uwarl_a/wam_joints_control/joint6/command
/uwarl_a/wam_joints_control/joint7/command
Hand
type: std_msgs/Float64
/uwarl_a/bhand_joints_control/joint1/command
/uwarl_a/bhand_joints_control/joint2/command
/uwarl_a/bhand_joints_control/joint3/command
/uwarl_a/bhand_joints_control/joint4/command
/uwarl_a/bhand_joints_control/joint5/command
/uwarl_a/bhand_joints_control/joint6/command
/uwarl_a/bhand_joints_control/joint7/command
/uwarl_a/bhand_joints_control/joint8/command
Summit
type: Twist
/uwarl_a/robotnik_base_control/cmd_vel
"""
class SummitMover(object):
# Constructor: All variables and functions with the self. keyword are initialised in the constructor
def __init__(self):
rospy.loginfo("Summit demo initialising...")
# Base controller
self.pub_base_velocity = rospy.Publisher(
'/uwarl_a/robotnik_base_control/cmd_vel',
Twist,
queue_size=1
)
# WAM Trajectory Controller
self.pub_wam_joint_position_array = rospy.Publisher(
'/uwarl_a/eff_based_pos_traj_controller/command',
JointTrajectory,
queue_size=1
)
# WAM Controller Position Controller
self.pub_wam_shoulder_joint_1_position = rospy.Publisher(
'/uwarl_a/wam_joints_control/joint1_position_controller/command',
Float64,
queue_size=1
)
Way-points
The first 3 values represent the XY planar position displacement and the angular displacement along the Z-axis of the MB from its previous orientation. Since I cannot provide that as a trajectory command (the MB is only commanded by velocity alone, unlike the manipulator joints which can be position controlled), I drop them and submit only the last 7 absolute joint angle positions as way-points (see in the `Publisher' script). The rest are here and are at an interval of 0.1 sec.
0.13742545135611,0.00528305610755619,0.056622592498019,-0.535529711734542,-1.5707963267949,-1.5707963267949,1.60775995357998,-1.0240410481183e-16,-1.08046726467338,0
0.137211843579129,0.00329429825814982,0.0566225924980168,-0.54229604302646,-1.5707963267949,-1.5707963267949,1.63306492562565,-1.02327387689195e-16,-1.10724292825506,0
0.136984737327069,0.00130736364584833,0.0566225924980191,-0.548864629549273,-1.5707963267949,-1.5707963267949,1.65803214634109,-1.02311126284883e-16,-1.13387858527562,0
0.136744148075668,-0.000677636605210048,0.0566225924980168,-0.555240438241973,-1.5707963267949,-1.5707963267949,1.68268105454751,-1.02352934192646e-16,-1.16038870761728,0
0.136490092215617,-0.00266059150212973,0.0566225924980191,-0.561427817774178,-1.5707963267949,-1.5707963267949,1.70702965367098,-1.02450789705296e-16,-1.18678695003648,0
0.136222587051151,-0.004641390190789,0.0566225924980191,-0.567430550999165,-1.5707963267949,-1.5707963267949,1.73109467117143,-1.02602996744507e-16,-1.21308625713988,0
0.135941650799055,-0.00661992196335856,0.0566225924980168,-0.573251899510292,-1.5707963267949,-1.5707963267949,1.7548916971415,-1.02808152494229e-16,-1.23929895742675,0
0.135647302587415,-0.00859607626582112,0.0566225924980191,-0.578894641469072,-1.5707963267949,-1.5707963267949,1.77843530540809,-1.03065120618768e-16,-1.2654368465625,0
0.135339562454232,-0.0105697427054565,0.056622592498019,-0.584361103653703,-1.5707963267949,-1.5707963267949,1.80173915986836,-1.03373009178406e-16,-1.29151126166607,0
Debugging
ROS Environment variables
┌─(~/UWARL_catkin_ws/src/waterloo_steel)──────────────────────────────────────────────(arnab@arl-System-Product-Name:pts/1)─┐
└─(15:14:35 on universal/ros1/arnab/session-jun-2023 ✹)──> env | grep ROS ──(Tue,Aug01)─┘
ROS_CORE_HOSTER=LOCAL-HOSTS
ROS_SUMMIT_IN_NETWORK_IP=192.168.1.11
ROS_WAM_IN_NETWORK_IP=192.168.1.10
ROS_DECK_IN_NETWORK_IP=192.168.1.15
ROS_JX_IN_NETWORK_PARALLEL_PC_IP=192.168.1.100
ROS_EXTERNAL_PC_IN_NETWORK_IP=192.168.1.100
ROS_CATKIN_WS=/home/arnab/UWARL_catkin_ws
ROS_DISTRO=noetic
PYTHONPATH_ROS=/usr/bin/python3
ROS_IP=localhost
ROS_HOSTNAME=localhost
ROS_MASTER_URI=http://localhost:11311/
ROS_ETC_DIR=/opt/ros/noetic/etc/ros
ROS_PACKAGE_PATH=/home/arnab/UWARL_catkin_ws/src/uwarl-barrett_wam_msgs:/home/arnab/UWARL_catkin_ws/src/uwarl-barrett_wam_hw/barrett_hand_node:/home/arnab/UWARL_catkin_ws/src/multimap_server_msgs:/home/arnab/UWARL_catkin_ws/src/uwarl-multimap_server:/home/arnab/UWARL_catkin_ws/src/uwarl-realsense_ros/realsense2_camera:/home/arnab/UWARL_catkin_ws/src/uwarl-realsense_ros/realsense2_description:/home/arnab/UWARL_catkin_ws/src/uwarl-robotnik_msgs:/home/arnab/UWARL_catkin_ws/src/uwarl-robot_localization_utils:/home/arnab/UWARL_catkin_ws/src/uwarl-robotnik_sensors:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_util/rosbag_recorder:/home/arnab/UWARL_catkin_ws/src/uwarl-realsense_ros/jx-rs4se:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/summit_xl_common:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_robot/summit_xl_controller:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/moveit/summit_xl_j2s6s200_moveit_config:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/moveit/summit_xl_j2s6s300_moveit_config:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/moveit/summit_xl_j2s7s300_moveit_config:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/summit_xl_localization:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/summit_xl_navigation:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_interface/summit_xl_pad:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/summit_xl_perception:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_common/summit_xl_robot_local_control:/home/arnab/UWARL_catkin_ws/src/system_monitor:/home/arnab/UWARL_catkin_ws/src/velodyne_simulator/velodyne_description:/home/arnab/UWARL_catkin_ws/src/velodyne_simulator/velodyne_gazebo_plugins:/home/arnab/UWARL_catkin_ws/src/velodyne_simulator/velodyne_simulator:/home/arnab/UWARL_catkin_ws/src/wagon_description:/home/arnab/UWARL_catkin_ws/src/wagon_tf_publisher:/home/arnab/UWARL_catkin_ws/src/uwarl-barrett_wam_hw/wam_node:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_demo/waterloo_steel_analyzer:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_viz/waterloo_steel_control:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_viz/waterloo_steel_description:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_viz/waterloo_steel_gazebo:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_viz/waterloo_steel_sim_bringup:/home/arnab/UWARL_catkin_ws/src/uwarl-summit_xl_robot/waterloo_steel_summit_bringup:/home/arnab/UWARL_catkin_ws/src/waterloo_steel/waterloo_steel_demo/waterloo_steel_supervisor:/opt/ros/noetic/share
ROS_PYTHON_VERSION=3
ROS_VERSION=1
ROS_ROOT=/opt/ros/noetic/share/ros
ROSLISP_PACKAGE_DIRECTORIES=/home/arnab/UWARL_catkin_ws/devel/share/common-lisp
Graphics card model and driver version
┌─(~/UWARL_catkin_ws/src/waterloo_steel)──────────────────────────────────────────────(arnab@arl-System-Product-Name:pts/1)─┐
└─(15:15:17 on universal/ros1/arnab/session-jun-2023 ✹)──> lspci | grep VGA ──(Tue,Aug01)─┘
01:00.0 VGA compatible controller: NVIDIA Corporation TU102 [GeForce RTX 2080 Ti Rev. A] (rev a1)
02:00.0 VGA compatible controller: NVIDIA Corporation TU102 [GeForce RTX 2080 Ti Rev. A] (rev a1)
Bag file
Recorded: "/gazebo/linkstates", "/uwarla/robotnikbasecontrol/odom", "/uwarla/robotnikbasecontrol/cmdvel", and "/uwarla/jointstates". Bag file.
Gazebo
┌─(~/UWARL_catkin_ws/src/waterloo_steel)────────────────────────────────────────────────(arnab@arl-System-Product-Name:pts/1)─┐
└─(15:23:25 on universal/ros1/arnab/session-jun-2023 ✹ ✭)──> gazebo --version ──(Tue,Aug01)─┘
Gazebo multi-robot simulator, version 11.11.0
Copyright (C) 2012 Open Source Robotics Foundation.
Released under the Apache 2 License.
http://gazebosim.org
Any idea what might be causing NaN values and how avoid this issue? The only way this might occur is if and inf is being divided by another inf (a wild guess).
In the meantime, I am planning to try and implement the pilz industrial motion package as it might be able to do what I wanted to. But since it also depends uon the `JointTrajectoryController', it may also give the same results.
Incidentally, even though the 7th joint (`uwarlawam/palmyawjoint') has been commanded not to move at all, it still shows some motion (Notice how the hand at the end of the manipulator wriggles around). It is unclear why so.
Thanks a lot for looking at this issue.
Asked by chiku00 on 2023-08-01 14:48:59 UTC
Comments
I found a similar problem description here, still with no solution.
Asked by chiku00 on 2023-08-02 07:06:39 UTC