Comments (5)
Ok just tested it out on a single joint (wrist_1). Up till 0.1 radians the action terminates successfully. Everything above result in the following message:
[ros2_control_node-1] [INFO] [1612449853.661850170] [ur_joint_trajectory_controller]: Received new action goal
[ros2_control_node-1] [INFO] [1612449853.661883916] [ur_joint_trajectory_controller]: Accepted new action goal
[ros2_control_node-1] [WARN] [1612449863.664272377] [ur_joint_trajectory_controller]: Aborted due to goal tolerance violation
For reproducing the error, I started the robot at home pose [0.0, -1.57, 0.0, -1.57, 0.0, 0.0] and send the following ActionGoal
control_msgs.action.FollowJointTrajectory_Goal(trajectory=trajectory_msgs.msg.JointTrajectory(
header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''),
joint_names=['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'],
points=[trajectory_msgs.msg.JointTrajectoryPoint(positions=[0.0, -1.57, 0.0, -1.3, 0.0, 0.0],
velocities=[],
accelerations=[],
effort=[],
time_from_start=builtin_interfaces.msg.Duration(sec=10, nanosec=0))]),
path_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)],
goal_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)],
goal_time_tolerance=builtin_interfaces.msg.Duration(sec=3, nanosec=0))
from universal_robots_ros2_driver.
As stated in teams #37 does not fix the Problem. It seems like the parameters are not correctly transfered into the JTC. Also I can't find them on the parameter server. I dug a little bit deeper into the error and it seems that the stopped_velocity_tolerance is the cause of the issue since. When the action fails the velocity is most of the time at around 0.1 (rad/s i guess). I increased it hardcoded in the tolerance.hpp which "fixed" the error. I will now have a closer look into the parameter server.
from universal_robots_ros2_driver.
Ok removed the hardcoded stuff and set the stopped_velocity_tolerance int the ur_ros2_control.yaml to 0.2 which works to prevent the error. However I guess the robot shouldn't have a velocity of 0.2 rad/s at the end of its trajectory.
from universal_robots_ros2_driver.
Ok just tested it out on a single joint (wrist_1). Up till 0.1 radians the action terminates successfully. Everything above result in the following message:
[ros2_control_node-1] [INFO] [1612449853.661850170] [ur_joint_trajectory_controller]: Received new action goal [ros2_control_node-1] [INFO] [1612449853.661883916] [ur_joint_trajectory_controller]: Accepted new action goal [ros2_control_node-1] [WARN] [1612449863.664272377] [ur_joint_trajectory_controller]: Aborted due to goal tolerance violation
For reproducing the error, I started the robot at home pose [0.0, -1.57, 0.0, -1.57, 0.0, 0.0] and send the following ActionGoal
control_msgs.action.FollowJointTrajectory_Goal(trajectory=trajectory_msgs.msg.JointTrajectory( header=std_msgs.msg.Header(stamp=builtin_interfaces.msg.Time(sec=0, nanosec=0), frame_id=''), joint_names=['elbow_joint', 'shoulder_lift_joint', 'shoulder_pan_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], points=[trajectory_msgs.msg.JointTrajectoryPoint(positions=[0.0, -1.57, 0.0, -1.3, 0.0, 0.0], velocities=[], accelerations=[], effort=[], time_from_start=builtin_interfaces.msg.Duration(sec=10, nanosec=0))]), path_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], goal_tolerance=[control_msgs.msg.JointTolerance(name='elbow_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_lift_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='shoulder_pan_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_1_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_2_joint', position=3.14, velocity=3.14, acceleration=3.14), control_msgs.msg.JointTolerance(name='wrist_3_joint', position=3.14, velocity=3.14, acceleration=3.14)], goal_time_tolerance=builtin_interfaces.msg.Duration(sec=3, nanosec=0))
Only place where that message can be outputed is here which actually depends on the outside_goal_tolerance
flag. This flag can then only be set here where the check for the last point is done.
If we take a closer look this function is setting the flag and it is based on these parameters.0
They contain goal and state tolerances which are on default zero.
This function will not change them if they are not specified - they remain zero.
Function that is actually using them is only checking tolerance error if the parameters are non-zero.
The quick fix would be to set stopped_velocity_tolerance
to zero.
from universal_robots_ros2_driver.
The issue has been created on the ros2_control.
from universal_robots_ros2_driver.
Related Issues (20)
- [ur_robot_driver] ur_ros2_control_node crashes when loading robot_description from a topic HOT 2
- Update the ROS node documentation
- Execution stuck or timeout when running ur_moveit_config HOT 2
- source build fails - ur_controllers HOT 1
- UR5 reverse interface keeps dropping in WSL2 HOT 2
- ur_robot_driver colcon build error HOT 1
- (ur_control.launch.py) custom description file from custom pkg HOT 7
- Compile error: str_cat not defined HOT 1
- Aborted during moving HOT 5
- I can't change velocity of robot when scaled joint trajectory control is operated. HOT 3
- Controlling Tool Output while using a newer version of the Robotiq Gripper URCap is not possible HOT 7
- unabele to execute path useing moveit on real hardwear HOT 3
- Issue name Issues while launching ur_robot_driver HOT 2
- The test_joint_trajectory_controller not work with ur_robot_driver for Humble but work with Foxy HOT 2
- arm move to slow in Cartesian movemants HOT 2
- Could we stop the External Control on the UR robot at runtime? HOT 2
- [GPIOController] - Use latched / transient topics for mode information HOT 1
- ProgramState.msg not used in GPIOController HOT 2
- Planning precise straight line movements for UR10e with ROS2 HOT 1
- [Feature] - Report a valid robot state (robot_mode/safety_state/program_state) when running with mock hardware. HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from universal_robots_ros2_driver.