search

Kinovarobotics / kinova-ros

ROS packages for Jaco2 and Mico robotic arms
BSD 3-Clause "New" or "Revised" License
376 stars 319 forks source link

some problems and errors when using moveit with j2s6s300 #384

Open Tzxtoiny opened 2 years ago

Tzxtoiny commented 2 years ago

Hello, when I try to control the arm using the moveit, I met some problems. I will explain my steps.

  1. I run the drive of the arm. 
    
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2s6s300
2. run the moveit launch file

roslaunch j2s6s300_moveit_config j2s6s300_demo.launch

the output in the terminal is below:

Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://harlab-NUC10i7FNH:39735/

SUMMARY

PARAMETERS

NODES / j2s6s300_gripper_command_action_server (kinova_driver/gripper_command_action_server) j2s6s300_joint_trajectory_action_server (kinova_driver/joint_trajectory_action_server) move_group (moveit_ros_move_group/move_group) rviz_harlab_NUC10i7FNH_16295_8078045931031624954 (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[j2s6s300_joint_trajectory_action_server-1]: started with pid [16310] process[j2s6s300_gripper_command_action_server-2]: started with pid [16311] process[move_group-3]: started with pid [16325] process[rviz_harlab_NUC10i7FNH_16295_8078045931031624954-4]: started with pid [16326] [ WARN] [1647528089.409765311]: MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default planning pipeline configuration [ WARN] [1647528089.410283658]: Falling back to using the the move_group node namespace (deprecated behavior). [ INFO] [1647528089.415432899]: Loading robot model 'j2s6s300'... [ WARN] [1647528089.415459087]: Skipping virtual joint 'world_to_base_virtual_joint' because its child frame 'root' does not match the URDF frame 'world' [ INFO] [1647528089.415471953]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1647528089.503076704]: rviz version 1.14.9 [ INFO] [1647528089.503116260]: compiled against Qt version 5.12.8 [ INFO] [1647528089.503125939]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1647528089.509168345]: Forcing OpenGl version 0. [ INFO] [1647528089.532534587]: Gripper_command__action_server receive feedback of trajectory state from topic: /trajectory_controller/state [ INFO] [1647528089.558582124]: Joint_trajectory_action_server receive feedback of trajectory state from topic: /trajectory_controller/state [ WARN] [1647528089.602575838]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...). [ INFO] [1647528089.604804914]: IK Using joint j2s6s300_link_1 -3.40282e+38 3.40282e+38 [ INFO] [1647528089.604824030]: IK Using joint j2s6s300_link_2 0.820305 5.46288 [ INFO] [1647528089.604832910]: IK Using joint j2s6s300_link_3 0.331613 5.95157 [ INFO] [1647528089.604841501]: IK Using joint j2s6s300_link_4 -3.40282e+38 3.40282e+38 [ INFO] [1647528089.604849188]: IK Using joint j2s6s300_link_5 0.523599 5.75959 [ INFO] [1647528089.604884944]: IK Using joint j2s6s300_link_6 -3.40282e+38 3.40282e+38 [ INFO] [1647528089.604896920]: Looking in common namespaces for param name: arm/position_only_ik [ INFO] [1647528089.605623115]: Looking in common namespaces for param name: arm/solve_type [ INFO] [1647528089.606259763]: Using solve type Speed [ INFO] [1647528089.621456133]: Start Follow_Joint_Trajectory_Action server! [ INFO] [1647528089.621480859]: Waiting for an plan execution (goal) from Moveit [ INFO] [1647528089.625504858]: Start Gripper_Command_Trajectory_Action server! [ INFO] [1647528089.773802076]: Stereo is NOT SUPPORTED [ INFO] [1647528089.773845900]: OpenGL device: Mesa Intel(R) UHD Graphics (CML GT2) [ INFO] [1647528089.773892952]: OpenGl version: 4.6 (GLSL 4.6) limited to GLSL 1.4 on Mesa system. [ INFO] [1647528089.900576987]: Publishing maintained planning scene on 'monitored_planning_scene' [ INFO] [1647528089.902260692]: Listening to 'joint_states' for joint states [ INFO] [1647528089.904741630]: Listening to '/attached_collision_object' for attached collision objects [ INFO] [1647528089.904763132]: Starting planning scene monitor [ INFO] [1647528089.905780297]: Listening to '/planning_scene' [ INFO] [1647528089.905801690]: Starting world geometry update monitor for collision objects, attached objects, octomap updates. [ INFO] [1647528089.906808334]: Listening to '/collision_object' [ INFO] [1647528089.907866231]: Listening to '/planning_scene_world' for planning scene world geometry [ INFO] [1647528089.908257281]: No 3D sensor plugin(s) defined for octomap updates [ INFO] [1647528090.159836293]: Loading planning pipeline '' [ INFO] [1647528090.188073363]: Using planning interface 'OMPL' [ INFO] [1647528090.190127387]: Param 'default_workspace_bounds' was not set. Using default value: 10 [ INFO] [1647528090.190349902]: Param 'start_state_max_bounds_error' was set to 0.1 [ INFO] [1647528090.190537687]: Param 'start_state_max_dt' was not set. Using default value: 0.5 [ INFO] [1647528090.190842208]: Param 'start_state_max_dt' was not set. Using default value: 0.5 [ INFO] [1647528090.191049707]: Param 'jiggle_fraction' was set to 0.05 [ INFO] [1647528090.191343855]: Param 'max_sampling_attempts' was not set. Using default value: 100 [ INFO] [1647528090.191374225]: Using planning request adapter 'Add Time Parameterization' [ INFO] [1647528090.191391363]: Using planning request adapter 'Fix Workspace Bounds' [ INFO] [1647528090.191403986]: Using planning request adapter 'Fix Start State Bounds' [ INFO] [1647528090.191417913]: Using planning request adapter 'Fix Start State In Collision' [ INFO] [1647528090.191463162]: Using planning request adapter 'Fix Start State Path Constraints' [ INFO] [1647528090.438944520]: Added FollowJointTrajectory controller for j2s6s300 [ INFO] [1647528090.822985678]: Added GripperCommand controller for j2s6s300_gripper [ INFO] [1647528090.823233389]: Returned 2 controllers in list [ INFO] [1647528090.859814204]: Trajectory execution is managing controllers [ INFO] [1647528090.859922705]: MoveGroup debug mode is ON Loading 'move_group/ApplyPlanningSceneService'... Loading 'move_group/ClearOctomapService'... Loading 'move_group/MoveGroupCartesianPathService'... Loading 'move_group/MoveGroupExecuteTrajectoryAction'... Loading 'move_group/MoveGroupGetPlanningSceneService'... Loading 'move_group/MoveGroupKinematicsService'... Loading 'move_group/MoveGroupMoveAction'... Loading 'move_group/MoveGroupPickPlaceAction'... Loading 'move_group/MoveGroupPlanService'... Loading 'move_group/MoveGroupQueryPlannersService'... Loading 'move_group/MoveGroupStateValidationService'... [ INFO] [1647528090.985545666]:

[ INFO] [1647528090.985582084]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner [ INFO] [1647528090.985595472]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1647528093.051991658]: Loading robot model 'j2s6s300'... [ WARN] [1647528093.052041747]: Skipping virtual joint 'world_to_base_virtual_joint' because its child frame 'root' does not match the URDF frame 'world' [ INFO] [1647528093.052067010]: No root/virtual joint specified in SRDF. Assuming fixed joint [ WARN] [1647528093.257344880]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...). [ INFO] [1647528093.260628183]: IK Using joint j2s6s300_link_1 -3.40282e+38 3.40282e+38 [ INFO] [1647528093.260666540]: IK Using joint j2s6s300_link_2 0.820305 5.46288 [ INFO] [1647528093.260685036]: IK Using joint j2s6s300_link_3 0.331613 5.95157 [ INFO] [1647528093.260702736]: IK Using joint j2s6s300_link_4 -3.40282e+38 3.40282e+38 [ INFO] [1647528093.260721202]: IK Using joint j2s6s300_link_5 0.523599 5.75959 [ INFO] [1647528093.260737548]: IK Using joint j2s6s300_link_6 -3.40282e+38 3.40282e+38 [ INFO] [1647528093.260758252]: Looking in common namespaces for param name: arm/position_only_ik [ INFO] [1647528093.262686423]: Looking in common namespaces for param name: arm/solve_type [ INFO] [1647528093.264162506]: Using solve type Speed [ INFO] [1647528093.592487055]: Starting planning scene monitor [ INFO] [1647528093.601584781]: Listening to '/move_group/monitored_planning_scene' [ INFO] [1647528093.801272402]: Constructing new MoveGroup connection for group 'arm' in namespace '' [ INFO] [1647528094.856596052]: Ready to take commands for planning group arm. [ INFO] [1647528094.856766436]: Looking around: no [ INFO] [1647528094.856848876]: Replanning: no

3. I move the robot to a different position in RVIZ and press "plan&execute".
![moveit](https://user-images.githubusercontent.com/62111393/158830300-91a016ca-e1a8-4a55-a007-79a3558c7ccd.png)
The arm didn't move. The output in the moveit terminial is

[ INFO] [1647528413.335611996]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1647528413.335855572]: Planning attempt 1 of at most 1 [ INFO] [1647528413.338332063]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [ INFO] [1647528413.339497662]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.339688758]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.339895674]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.340121533]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.350860032]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.351040156]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.351316788]: arm/arm: Created 5 states (3 start + 2 goal) [ INFO] [1647528413.351480814]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.351627288]: ParallelPlan::solve(): Solution found by one or more threads in 0.012845 seconds [ INFO] [1647528413.351857962]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.351964360]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.352018463]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.352074763]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.352701250]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.352863047]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.352946897]: arm/arm: Created 5 states (2 start + 3 goal) [ INFO] [1647528413.353117460]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.353248863]: ParallelPlan::solve(): Solution found by one or more threads in 0.001515 seconds [ INFO] [1647528413.353455600]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.353508000]: arm/arm: Starting planning with 1 states already in datastructure [ INFO] [1647528413.354221889]: arm/arm: Created 5 states (2 start + 3 goal) [ INFO] [1647528413.354254021]: arm/arm: Created 4 states (2 start + 2 goal) [ INFO] [1647528413.354349061]: ParallelPlan::solve(): Solution found by one or more threads in 0.001016 seconds [ INFO] [1647528413.356373053]: SimpleSetup: Path simplification took 0.001982 seconds and changed from 3 to 2 states [ INFO] [1647528413.380359370]: Joint_trajectory_action_server received goal! [ INFO] [1647528413.380609860]: Joint_trajectory_action_server accepted goal! [ INFO] [1647528413.380811868]: Joint_trajectory_action_server published goal via command publisher! [ WARN] [1647528414.454311455]: Aborting because we wound up outside the goal constraints [ INFO] [1647528414.454861663]: Controller j2s6s300 successfully finished [ WARN] [1647528414.455061719]: Controller handle j2s6s300 reports status ABORTED [ INFO] [1647528414.455269035]: Completed trajectory execution with status ABORTED ... [ INFO] [1647528414.476270631]: ABORTED: Solution found but controller failed during execution

The output in the arm bringup terminial is

[ INFO] [1647528381.050886160]: Trajectory controller Receive trajectory with points number: 3 [DEBUG] [1647528381.075697540]: Trying to publish message of type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] on a publisher with type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] [ INFO] [1647528381.075826719]: Moved to point 0 [ INFO] [1647528381.562635624]: Moved to point 1 [ INFO] [1647528382.051809024]: Moved to point 2


4. Then I run the demo file in the '/kinova-ros-noetic-devel/kinova_m
oveit/kinova_arm_moveit_demo/src/motion_plan.cpp' by the command

rosrun kinova_arm_moveit_demo motion_plan

, then I get the error in the terminal !!

[ INFO] [1647528674.828347270]: Loading robot model 'j2s6s300'... [ WARN] [1647528674.831376580]: Skipping virtual joint 'world_to_base_virtual_joint' because its child frame 'root' does not match the URDF frame 'world' [ INFO] [1647528674.831486866]: No root/virtual joint specified in SRDF. Assuming fixed joint [ WARN] [1647528675.064525516]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...). [ INFO] [1647528675.067180366]: IK Using joint j2s6s300_link_1 -3.40282e+38 3.40282e+38 [ INFO] [1647528675.067204667]: IK Using joint j2s6s300_link_2 0.820305 5.46288 [ INFO] [1647528675.067215304]: IK Using joint j2s6s300_link_3 0.331613 5.95157 [ INFO] [1647528675.067226586]: IK Using joint j2s6s300_link_4 -3.40282e+38 3.40282e+38 [ INFO] [1647528675.067238196]: IK Using joint j2s6s300_link_5 0.523599 5.75959 [ INFO] [1647528675.067248548]: IK Using joint j2s6s300_link_6 -3.40282e+38 3.40282e+38 [ INFO] [1647528675.067260552]: Looking in common namespaces for param name: arm/position_only_ik [ INFO] [1647528675.068248508]: Looking in common namespaces for param name: arm/solve_type [ INFO] [1647528675.069130439]: Using solve type Speed [ INFO] [1647528676.208830977]: Ready to take commands for planning group arm. [ INFO] [1647528676.222042786]: Reference frame: world [ INFO] [1647528676.222149380]: Reference frame: j2s6s300_end_effector [ INFO] [1647528676.301755123]: Visualizing plan 1 (pose goal) [ INFO] [1647528681.301951406]: Visualizing plan 1 (again) [ INFO] [1647528687.310602988]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds. Check clock synchronization if your are running ROS across multiple machines! [ERROR] [1647528687.310740451]: Failed to fetch current robot state [ INFO] [1647528688.311026840]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds. Check clock synchronization if your are running ROS across multiple machines! [ERROR] [1647528688.311185578]: Failed to fetch current robot state


The error is "[ERROR] [1647528687.310740451]: Failed to fetch current robot state
[ INFO] [1647528688.311026840]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1647528688.311185578]: Failed to fetch current robot state**"
felixmaisonneuve commented 2 years ago

Hi @Tzxtoiny,

Thank you for this very detailed issue.

Do you also have the error when running the motion_plan example if you do not execute the trajectory in moveit before?

I feel like your third step, where you plan&execute a moveit trajectory, puts the arm/moveit in an error state (which is not normal). The motion_plan is then unable to proceed normally. I will assume there is no problem with it if you are able to run it without using moveit's motion planning first.

For the Motion Planning that is not moving the arm, I think the Aborting because we wound up outside the goal constraints is "normal". By default, I think the angle constaint is 0.0001 degrees for each joint (which is impossible to obtain on this arm), so the warning actually appears all the time, but I would need to verify all of this because I am not sure. If I recall correctly, I think I never bothered to fix the constaints because it doesn't impact anything (except that it prints a warning at the end of the trajectory) and the arm should move anyway. In your case, the arm is not moving at all if I understand correctly. From your screenshot, the movement your are trying to do is somewhat small. Have you tried to use the motion planning for a bigger movement? Are you able to use MoveIt's Motion planning at all or is your arm always staying at the same place regardless of the trajectory?

Regards, Felix

Tzxtoiny commented 2 years ago

Hi @Tzxtoiny,

Thank you for this very detailed issue.

Do you also have the error when running the motion_plan example if you do not execute the trajectory in moveit before?

I feel like your third step, where you plan&execute a moveit trajectory, puts the arm/moveit in an error state (which is not normal). The motion_plan is then unable to proceed normally. I will assume there is no problem with it if you are able to run it without using moveit's motion planning first.

For the Motion Planning that is not moving the arm, I think the Aborting because we wound up outside the goal constraints is "normal". By default, I think the angle constaint is 0.0001 degrees for each joint (which is impossible to obtain on this arm), so the warning actually appears all the time, but I would need to verify all of this because I am not sure. If I recall correctly, I think I never bothered to fix the constaints because it doesn't impact anything (except that it prints a warning at the end of the trajectory) and the arm should move anyway. In your case, the arm is not moving at all if I understand correctly. From your screenshot, the movement your are trying to do is somewhat small. Have you tried to use the motion planning for a bigger movement? Are you able to use MoveIt's Motion planning at all or is your arm always staying at the same place regardless of the trajectory?

Regards, Felix

Hello, tried what you said, but unfortunately, the error still occurred. This time I didn't move the robot in the RVIZ, and run the code 

rosrun kinova_arm_moveit_demo motion_plan

after run the drive of robot and the launch file of moveit. Then the output in the terminal is :

[ INFO] [1647586842.967091618]: Loading robot model 'j2s6s300'...
[ WARN] [1647586842.970002375]: Skipping virtual joint 'world_to_base_virtual_joint' because its child frame 'root' does not match the URDF frame 'world'
[ INFO] [1647586842.970112601]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1647586843.207638523]: IK plugin for group 'arm' relies on deprecated API. Please implement initialize(RobotModel, ...).
[ INFO] [1647586843.210123706]: IK Using joint j2s6s300_link_1 -3.40282e+38 3.40282e+38
[ INFO] [1647586843.210160063]: IK Using joint j2s6s300_link_2 0.820305 5.46288
[ INFO] [1647586843.210173656]: IK Using joint j2s6s300_link_3 0.331613 5.95157
[ INFO] [1647586843.210187184]: IK Using joint j2s6s300_link_4 -3.40282e+38 3.40282e+38
[ INFO] [1647586843.210199996]: IK Using joint j2s6s300_link_5 0.523599 5.75959
[ INFO] [1647586843.210212606]: IK Using joint j2s6s300_link_6 -3.40282e+38 3.40282e+38
[ INFO] [1647586843.210228387]: Looking in common namespaces for param name: arm/position_only_ik
[ INFO] [1647586843.211209311]: Looking in common namespaces for param name: arm/solve_type
[ INFO] [1647586843.211989965]: Using solve type Speed
[ INFO] [1647586844.344764118]: Ready to take commands for planning group arm.
[ INFO] [1647586844.354839645]: Reference frame: world
[ INFO] [1647586844.354976843]: Reference frame: j2s6s300_end_effector
[ INFO] [1647586844.523811257]: Visualizing plan 1 (pose goal) 
[ INFO] [1647586849.524078668]: Visualizing plan 1 (again)
[ INFO] [1647586855.532387985]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1647586855.532537234]: Failed to fetch current robot state
[ INFO] [1647586856.532799023]: Didn't receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ERROR] [1647586856.532905741]: Failed to fetch current robot state

It seems that moveit can't get the current position of the arm. moveit2

As for the step3 , I try to move the arm in RIVIZ far away from its home position, the arm still didn't move at all. moveit3

felixmaisonneuve commented 2 years ago

It seems like the example is looking for the robot state (/joint_states), but cannot find it. Normally this should be published on topic /j2s6s300_driver/out/joint_state https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_bringup/launch/kinova_robot.launch#L29-L36

Then this topic is passed as an argument to the move_group (which is used by moveit and the motion_plan example) https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/j2s6s300_demo.launch#L29-L36

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/move_group_j2s6s300.launch#L73

Did you do any modifications to these files and/or other launch files?

This is because the driver connects to the arm and publishes directly on this topic (/j2s6s300_driver/out/joint_state) https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L197-L198

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L698

The motion_plan example is a bit different from the other moveit example, but I think it should work nonetheless. I am unable to test at the moment but I will make sure to get access to arm and test on my side to see if the motion_plan example is working at all or if it is a problem with your specific setup. Maybe you could try the other example and see if you get a similar error?

Best, Felix

gbcarlos commented 2 years ago

Hi,

I am getting same errors as described here. In my case the arm is moving but never reaches the position it plans to, instead aborts seemingly before the last point of trajectory. This is not due to joint limits since I can execute the plan again and the arm will move closer to the end goal (not reaching exactly though). My bringup terminal looks the same as @Tzxtoiny and in my j2s6s300_demo.launch terminal I also get the same errors:

[ WARN] [1647615309.149574057]: Aborting because we wound up outside the goal constraints
[ INFO] [1647615309.150508141]: Controller 'j2s6s300' successfully finished
[ WARN] [1647615309.150732114]: Controller handle j2s6s300 reports status ABORTED
[ INFO] [1647615309.150932863]: Completed trajectory execution with status ABORTED ...
[ INFO] [1647615309.151252502]: Execution completed: ABORTED

Changing goal time and goal constraints for each joint did not resolve the issue.

Executing the motion_plan file also didn't work for me with the same errors, which might be due to the remapping of joint states as you mentioned @felixmaisonneuve. Since my arm is moving I am not going to look further into the motion plan, however it would be nice to know where the aborting behaviour comes from.

Tzxtoiny commented 2 years ago

It seems like the example is looking for the robot state (/joint_states), but cannot find it. Normally this should be published on topic /j2s6s300_driver/out/joint_state

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_bringup/launch/kinova_robot.launch#L29-L36

Then this topic is passed as an argument to the move_group (which is used by moveit and the motion_plan example)

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/j2s6s300_demo.launch#L29-L36

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/move_group_j2s6s300.launch#L73

Did you do any modifications to these files and/or other launch files?

This is because the driver connects to the arm and publishes directly on this topic (/j2s6s300_driver/out/joint_state)

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L197-L198

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L698

The motion_plan example is a bit different from the other moveit example, but I think it should work nonetheless. I am unable to test at the moment but I will make sure to get access to arm and test on my side to see if the motion_plan example is working at all or if it is a problem with your specific setup. Maybe you could try the other example and see if you get a similar error?

Best, Felix @felixmaisonneuve @gbcarlos , hi! I tried to move the arm in another computer which is ubuntu 18.04(ROS melodic), the arm can execute the motion plan in RVIZ, but when running the motion plan file, it still get the same error. I found that in my move_group_j2s6s300.launch file(Ubuntu 20.04 noetic), the code is different from the current code in GITHUB. MAYBE the error is due to the difference, but I didn't modify the code before. I will change the code to test it again to see if it will be solved. Then I will give you the feedback. Thank you!

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />      

    <remap unless="$(arg fake_execution)" from="/joint_states" to="$(arg joint_states_ns)" />
  </node>

</launch>

And I also wonder why in RVIZ the plan is aborted, because in bring up terminal, the output means that the trajectory have been sent to the trajectory controller. The output in the arm bringup terminial is like:

[ INFO] [1647528381.050886160]: Trajectory controller Receive trajectory with points number: 3 [DEBUG] [1647528381.075697540]: Trying to publish message of type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] on a publisher with type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] [ INFO] [1647528381.075826719]: Moved to point 0 [ INFO] [1647528381.562635624]: Moved to point 1 [ INFO] [1647528382.051809024]: Moved to point 2

Tzxtoiny commented 2 years ago

It seems like the example is looking for the robot state (/joint_states), but cannot find it. Normally this should be published on topic /j2s6s300_driver/out/joint_state https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_bringup/launch/kinova_robot.launch#L29-L36

Then this topic is passed as an argument to the move_group (which is used by moveit and the motion_plan example) https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/j2s6s300_demo.launch#L29-L36

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_moveit/robot_configs/j2s6s300_moveit_config/launch/move_group_j2s6s300.launch#L73

Did you do any modifications to these files and/or other launch files? This is because the driver connects to the arm and publishes directly on this topic (/j2s6s300_driver/out/joint_state) https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L197-L198

https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/kinova_arm.cpp#L698

The motion_plan example is a bit different from the other moveit example, but I think it should work nonetheless. I am unable to test at the moment but I will make sure to get access to arm and test on my side to see if the motion_plan example is working at all or if it is a problem with your specific setup. Maybe you could try the other example and see if you get a similar error? Best, Felix @felixmaisonneuve @gbcarlos , hi! I tried to move the arm in another computer which is ubuntu 18.04(ROS melodic), the arm can execute the motion plan in RVIZ, but when running the motion plan file, it still get the same error. I found that in my move_group_j2s6s300.launch file(Ubuntu 20.04 noetic), the code is different from the current code in GITHUB. MAYBE the error is due to the difference, but I didn't modify the code before. I will change the code to test it again to see if it will be solved. Then I will give you the feedback. Thank you!

    <!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
    <param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
    <param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />      

    <remap unless="$(arg fake_execution)" from="/joint_states" to="$(arg joint_states_ns)" />
  </node>

</launch>

And I also wonder why in RVIZ the plan is aborted, because in bring up terminal, the output means that the trajectory have been sent to the trajectory controller. The output in the arm bringup terminial is like:

[ INFO] [1647528381.050886160]: Trajectory controller Receive trajectory with points number: 3 [DEBUG] [1647528381.075697540]: Trying to publish message of type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] on a publisher with type [kinova_msgs/JointVelocity/bf16c4e5154848a0767d1aaab0a4796b] [ INFO] [1647528381.075826719]: Moved to point 0 [ INFO] [1647528381.562635624]: Moved to point 1 [ INFO] [1647528382.051809024]: Moved to point 2

Hi, @felixmaisonneuve I modified the code as you said, but nothing changed. The problems in step3 and step4 still there. I also found my code is the same as the code in the noetic branch and a bit from yours.

felixmaisonneuve commented 2 years ago

I will to test all of this on real arm somewhere this week. I will keep you two updated

Tzxtoiny commented 2 years ago

I will to test all of this on real arm somewhere this week. I will keep you two updated

Hi, @felixmaisonneuve ！ I think I know the reason of why in moveit the arm seemed not to move, it seems that the speed range is limited nearly to zeto when using the joint velocity control of the arm. When the moveit plan a trajectory it send the points to the topic " /j2s6s300_driver/trajectory_controller/command", and the "kinova_joint_trajectory_controller" takes out the velocity command in it and send it to the service for velocity control of the robotic arm. Just in the "kinova_joint_trajectory_controller.cpp",

    pub_joint_velocity_ = pn.advertise<kinova_msgs::JointVelocity>("in/joint_velocity", 2);

and

    if (traj_command_points_index_ <  kinova_angle_command_.size() && ros::ok())
    {
        joint_velocity_msg.joint1 = kinova_angle_command_[traj_command_points_index_].Actuator1;
        joint_velocity_msg.joint2 = kinova_angle_command_[traj_command_points_index_].Actuator2;
        joint_velocity_msg.joint3 = kinova_angle_command_[traj_command_points_index_].Actuator3;
        joint_velocity_msg.joint4 = kinova_angle_command_[traj_command_points_index_].Actuator4;
        joint_velocity_msg.joint5 = kinova_angle_command_[traj_command_points_index_].Actuator5;
        joint_velocity_msg.joint6 = kinova_angle_command_[traj_command_points_index_].Actuator6;
        joint_velocity_msg.joint7 = kinova_angle_command_[traj_command_points_index_].Actuator7;

        pub_joint_velocity_.publish(joint_velocity_msg);

For some reason, there is a problem with the joint velocity control of the robotic arm, its joint velocity is limited to very small, almost the same as 0. BUT I didn't modify the limit of joint velocity in "robot_parameters.yaml", I even change it to a bigger number, but it seems not to work, here is my "robot_parameters.yaml".

# Set this parameter to use a specific arm on your system    --> 
# serial_number: PJ00000001030703130

# Joint speed limit for joints 1, 2, 3
jointSpeedLimitParameter1: 50

# Joint speed limit for joints 4, 5, 6
jointSpeedLimitParameter2: 50

# payload: [COM COMx COMy COMz]
#payload: [0, 0, 0, 0]

connection_type: USB # Ethernet or USB

# Ethernet connection parameters
ethernet: {
  local_machine_IP: 192.168.100.100,
  subnet_mask: 255.255.255.0,
  local_cmd_port: 25000,
  local_broadcast_port: 25025
}

#Torque control parameters
#Do not change these parameters unless you want to change torque control behavior
torque_parameters: {
      # publish_torque_with_gravity_compensation: false,
      publish_torque_with_gravity_compensation: true,
      use_estimated_COM_parameters: false,
      # use_estimated_COM_parameters: true,
    # if torque min/max sepecified, all min/max values need to be specified
    #  torque_min: [80, 80, 80, 80, 80, 80, 80],
    #  torque_max: [90, 90, 90, 90, 90, 90, 90],
    # Decides velocity threshold at which robot switches torque to position control (between 0 and 1)
    # safety_factor: 1,
    # COM parameters
    # order [m1,m2,...,m7,x1,x2,...,x7,y1,y2,...y7,z1,z2,...z7]
    # com_parameters: [0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0, 0,0,0,0,0,0,0]
}

I set the speed limit to both 50deg/s, and in the bring up terminal, the parameters are read and set successfully. BUT obviously this modification did not work, and the robotic arm can still only move at a speed close to 0. 

SUMMARY
========

PARAMETERS
 * /j2s6s300_driver/connection_type: USB
 * /j2s6s300_driver/ethernet/local_broadcast_port: 25025
 * /j2s6s300_driver/ethernet/local_cmd_port: 25000
 * /j2s6s300_driver/ethernet/local_machine_IP: 192.168.100.100
 * /j2s6s300_driver/ethernet/subnet_mask: 255.255.255.0
 * /j2s6s300_driver/jointSpeedLimitParameter1: 50
 * /j2s6s300_driver/jointSpeedLimitParameter2: 50
 * /j2s6s300_driver/robot_name: j2s6s300
 * /j2s6s300_driver/robot_type: j2s6s300
 * /j2s6s300_driver/serial_number: not_set
 * /j2s6s300_driver/status_interval_seconds: 0.02
 * /j2s6s300_driver/torque_parameters/publish_torque_with_gravity_compensation: True
 * /j2s6s300_driver/torque_parameters/use_estimated_COM_parameters: False
 * /j2s6s300_driver/use_jaco_v1_fingers: False
 * /robot_description: <?xml version="1....
 * /rosdistro: noetic
 * /rosversion: 1.15.11

I also tried to use the command to test the joint velocity control, but it also didn't work, the joint 1 and joint 4 moved very slowly, just like a bit more than 0. If can, I will upload a vedio.

rostopic pub -r 100 /j2s6s300_driver/in/joint_velocity kinova_msgs/JointVelocity "{joint1: 5.0, joint2: 0.0, joint3: 0.0, joint4: 10.0, joint5: 0.0, joint6: 0.0}"

My ros version is noetic, I tried to download the latest corresponding version code on github, but it still didn't work. I also used the same a second j2s6s300 in our lab to test, but the results are the same. But in the ros melodic I test in another computer, the velocity control can work.

I want to know what is the reason for this? This problem really bothers me. THANK YOU!

Tzxtoiny commented 2 years ago

https://user-images.githubusercontent.com/62111393/159734071-5f123119-ff8b-42bb-80c6-9efc56ff1150.MOV

felixmaisonneuve commented 2 years ago

Hi @Tzxtoiny,

I was able to test a bit, but the arm I was using had very stiff joints (the arm couldn't even fall with its own weight when powered off) and the joints were not calibrated. When putting all joints to 180 degrees, you could clearly see the arm was not perfectly straight. I had to SetZeroPosition to all joints using Development Center and reboot the arm.

Because of this issue and the stiff joints, I was unable to test properly, so I will try to test with another arm soon.

For your joints 1 and 4 that moves very slowly, there might be a velocity limit set for those. I am pretty sure you can check this in Development Center. A "Reset Factory Settings" should get rid of them (if this is a the issue). I would try that first and check again if they are slow.

The limits used that leads to the error message Aborting because we wound up outside the goal constraints are defined here : https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L36-L48

The error message is in the same file : https://github.com/Kinovarobotics/kinova-ros/blob/93f66822ec073827eac5de59ebc2a3c5f06bf17f/kinova_driver/src/joint_trajectory_action/joint_trajectory_action_server.cpp#L235-L269

I am sorry I cannot help you more at the moment, I am quite busy at the moment I will tackle this issue as soon as possible

Regards, Felix