No movement on real robot

GianlucaCerilli commented 6 years ago

Hi,

I have a j2n6s300 Jaco robot and I would like to move it to a certain goal pose. I followed these steps (with the robot in home position):

roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2n6s300
roslaunch j2n6s300_moveit_config j2n6s300_demo.launch
selected current (start state) and update in rviz
selected vertical (goal state) and update in rviz
selected plan (and I see the grey manipulator moving)
selected execute At this step I don't see any movement, neither in the simulator nor in the real robot, but on the console it is written:

[ INFO] [1539620921.508211080]: Received new trajectory execution service request... [ INFO] [1539620921.597989112]: Joint_trajectory_action_server received goal! [ INFO] [1539620921.598172281]: Joint_trajectory_action_server accepted goal! [ INFO] [1539620921.598330803]: Joint_trajectory_action_server published goal via command publisher! [ INFO] [1539620928.396945425]: Completed trajectory execution with status SUCCEEDED ... [ INFO] [1539620928.397287469]: Execution completed: SUCCEEDED

Can you tell me why the real robot is not moving? Thank you very much

P.S. After roslaunch j2n6s300_moveit_config j2n6s300_demo.launch I have some warnings on some deprecated functions and an error:

[ERROR] [1539621324.234758587]: SpinnerMonitor: single-threaded spinner after multi-threaded one(s).Attempt to spin a callback queue from two spinners, one of them being single-threaded. In the future this will throw an exception! Only allowed for backwards compatibility.

These are for your information, but I don't think they are related with this problem. Thank you again

nsaif-kinova commented 6 years ago

Hello GianlucaCerilli, I tried the same steps on my robot and I am able to plan and execute a a goal in MoveIt, the robot is also following the same movement. Are you using ROS Indigo? Sincerely,

GianlucaCerilli commented 6 years ago

Hi nsaif-kinova, Thank you for your reply. I am using Ubuntu 16.04 with Ros Kinetic. But moveit and all the rest is correctly working with it.. So I don't know what could be the problem.

martine1406 commented 6 years ago

do you have any output in your terminal where you launch roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2n6s300

GianlucaCerilli commented 6 years ago

Hi Martine,

Tomorrow morning I can send a screen of the two terminals. By the way, I have no output in that terminal after running moveit with rviz

GianlucaCerilli commented 6 years ago

Sorry for the long message, but here you can read the messages on the two terminals.

This is the terminal for the roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2n6s300 :

... logging to /home/gianluca/.ros/log/d9ad25ba-d114-11e8-86a4-94b86df88095/roslaunch-gianluca-Precision-5530-11954.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

xacro: Traditional processing is deprecated. Switch to --inorder processing! To check for compatibility of your document, use option --check-order. For more infos, see http://wiki.ros.org/xacro#Processing_Order xacro.py is deprecated; please use xacro instead started roslaunch server http://gianluca-Precision-5530:40515/

Summary

Parameters

/j2n6s300_driver/connection_type: USB

/j2n6s300_driver/ethernet/local_broadcast_port: 25025

/j2n6s300_driver/ethernet/local_cmd_port: 25000

/j2n6s300_driver/ethernet/local_machine_IP: 192.168.100.100

/j2n6s300_driver/ethernet/subnet_mask: 255.255.255.0

/j2n6s300_driver/jointSpeedLimitParameter1: 10

/j2n6s300_driver/jointSpeedLimitParameter2: 20

/j2n6s300_driver/robot_name: j2n6s300

/j2n6s300_driver/robot_type: j2n6s300

/j2n6s300_driver/serial_number: not_set

/j2n6s300_driver/torque_parameters/publish_torque_with_gravity_compensation: False

/j2n6s300_driver/torque_parameters/use_estimated_COM_parameters: False

/kinova_number_of_robots: 2

/kinova_robots: [{'serial': 'PJ00...

/robot_description: <?xml version="1....

/rosdistro: kinetic

/rosversion: 1.12.14

Nodes / j2n6s300_driver (kinova_driver/kinova_arm_driver) j2n6s300_state_publisher (robot_state_publisher/robot_state_publisher)

ROS_MASTER_URI=http://localhost:11311

process[j2n6s300_driver-1]: started with pid [11974] process[j2n6s300_state_publisher-2]: started with pid [11975] [ INFO] [1539675088.046817561]: kinova_robotType is j2n6s300. [ INFO] [1539675088.047142567]: kinova_robotName is j2n6s300. [ INFO] [1539675088.049821112]: Initializing Kinova USB API (header version: 50300, library version: 5.2.0) [ INFO] [1539675088.206368194]: Found 1 device(s), using device at index 0 (model: Jaco V2 Service, serial number: PJ00900006161110001, code version: 393494, code revision: 6) [ INFO] [1539675088.280301818]: Initializing fingers...this will take a few seconds and the fingers should open completely [ INFO] [1539675088.431798079]: The arm is ready to use. j2n6s300_joint_1 j2n6s300_joint_2 j2n6s300_joint_3 j2n6s300_joint_4 j2n6s300_joint_5 j2n6s300_joint_6

And this is the terminal for the roslaunch j2n6s300_moveit_config j2n6s300_demo.launch :

... logging to /home/gianluca/.ros/log/d9ad25ba-d114-11e8-86a4-94b86df88095/roslaunch-gianluca-Precision-5530-12246.log Checking log directory for disk usage. This may take awhile. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

xacro: Traditional processing is deprecated. Switch to --inorder processing! To check for compatibility of your document, use option --check-order. For more infos, see http://wiki.ros.org/xacro#Processing_Order xacro.py is deprecated; please use xacro instead started roslaunch server http://gianluca-Precision-5530:45977/

Summary

Parameters

/controller_list: [{'default': True...

/move_group/allow_trajectory_execution: True

/move_group/arm/planner_configs: ['SBLkConfigDefau...

/move_group/capabilities: move_group/MoveGr...

/move_group/controller_list: [{'default': True...

/move_group/gripper/planner_configs: ['SBLkConfigDefau...

/move_group/jiggle_fraction: 0.05

/move_group/max_range: 5.0

/move_group/max_safe_path_cost: 1

/move_group/moveit_controller_manager: moveit_simple_con...

/move_group/moveit_manage_controllers: True

/move_group/octomap_resolution: 0.025

/move_group/planner_configs/BKPIECEkConfigDefault/border_fraction: 0.9

/move_group/planner_configs/BKPIECEkConfigDefault/failed_expansion_score_factor: 0.5

/move_group/planner_configs/BKPIECEkConfigDefault/min_valid_path_fraction: 0.5

/move_group/planner_configs/BKPIECEkConfigDefault/range: 0.0

/move_group/planner_configs/BKPIECEkConfigDefault/type: geometric::BKPIECE

/move_group/planner_configs/ESTkConfigDefault/goal_bias: 0.05

/move_group/planner_configs/ESTkConfigDefault/range: 0.0

/move_group/planner_configs/ESTkConfigDefault/type: geometric::EST

/move_group/planner_configs/KPIECEkConfigDefault/border_fraction: 0.9

/move_group/planner_configs/KPIECEkConfigDefault/failed_expansion_score_factor: 0.5

/move_group/planner_configs/KPIECEkConfigDefault/goal_bias: 0.05

/move_group/planner_configs/KPIECEkConfigDefault/min_valid_path_fraction: 0.5

/move_group/planner_configs/KPIECEkConfigDefault/range: 0.0

/move_group/planner_configs/KPIECEkConfigDefault/type: geometric::KPIECE

/move_group/planner_configs/LBKPIECEkConfigDefault/border_fraction: 0.9

/move_group/planner_configs/LBKPIECEkConfigDefault/min_valid_path_fraction: 0.5

/move_group/planner_configs/LBKPIECEkConfigDefault/range: 0.0

/move_group/planner_configs/LBKPIECEkConfigDefault/type: geometric::LBKPIECE

/move_group/planner_configs/PRMkConfigDefault/max_nearest_neighbors: 10

/move_group/planner_configs/PRMkConfigDefault/type: geometric::PRM

/move_group/planner_configs/PRMstarkConfigDefault/type: geometric::PRMstar

/move_group/planner_configs/RRTConnectkConfigDefault/range: 0.0

/move_group/planner_configs/RRTConnectkConfigDefault/type: geometric::RRTCon...

/move_group/planner_configs/RRTkConfigDefault/goal_bias: 0.05

/move_group/planner_configs/RRTkConfigDefault/range: 0.0

/move_group/planner_configs/RRTkConfigDefault/type: geometric::RRT

/move_group/planner_configs/RRTstarkConfigDefault/delay_collision_checking: 1

/move_group/planner_configs/RRTstarkConfigDefault/goal_bias: 0.05

/move_group/planner_configs/RRTstarkConfigDefault/range: 0.0

/move_group/planner_configs/RRTstarkConfigDefault/type: geometric::RRTstar

/move_group/planner_configs/SBLkConfigDefault/range: 0.0

/move_group/planner_configs/SBLkConfigDefault/type: geometric::SBL

/move_group/planner_configs/TRRTkConfigDefault/frountierNodeRatio: 0.1

/move_group/planner_configs/TRRTkConfigDefault/frountier_threshold: 0.0

/move_group/planner_configs/TRRTkConfigDefault/goal_bias: 0.05

/move_group/planner_configs/TRRTkConfigDefault/init_temperature: 10e-6

/move_group/planner_configs/TRRTkConfigDefault/k_constant: 0.0

/move_group/planner_configs/TRRTkConfigDefault/max_states_failed: 10

/move_group/planner_configs/TRRTkConfigDefault/min_temperature: 10e-10

/move_group/planner_configs/TRRTkConfigDefault/range: 0.0

/move_group/planner_configs/TRRTkConfigDefault/temp_change_factor: 2.0

/move_group/planner_configs/TRRTkConfigDefault/type: geometric::TRRT

/move_group/planning_plugin: ompl_interface/OM...

/move_group/planning_scene_monitor/publish_geometry_updates: True

/move_group/planning_scene_monitor/publish_planning_scene: True

/move_group/planning_scene_monitor/publish_state_updates: True

/move_group/planning_scene_monitor/publish_transforms_updates: True

/move_group/request_adapters: default_planner_r...

/move_group/start_state_max_bounds_error: 0.1

/move_group/trajectory_execution/allowed_goal_duration_margin: 0.5

/pick_place_demo/arm/kinematics_solver_timeout: 0.05

/pick_place_demo/arm/solve_type: Manipulation2

/robot_description: <?xml version="1....

/robot_description_kinematics/arm/kinematics_solver: trac_ik_kinematic...

/robot_description_kinematics/arm/kinematics_solver_attempts: 3

/robot_description_kinematics/arm/kinematics_solver_search_resolution: 0.005

/robot_description_kinematics/arm/kinematics_solver_timeout: 0.005

/robot_description_kinematics/arm/solve_type: Distance

/robot_description_planning/joint_limits/j2n6s300_joint_1/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_1/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_1/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_1/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_2/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_2/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_2/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_2/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_3/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_3/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_3/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_3/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_4/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_4/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_4/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_4/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_5/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_5/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_5/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_5/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_6/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_6/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_6/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_6/max_velocity: 0.35

/robot_description_planning/joint_limits/j2n6s300_joint_finger_1/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_1/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_1/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_1/max_velocity: 5

/robot_description_planning/joint_limits/j2n6s300_joint_finger_2/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_2/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_2/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_2/max_velocity: 5

/robot_description_planning/joint_limits/j2n6s300_joint_finger_3/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_3/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_3/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_3/max_velocity: 5

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_1/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_1/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_1/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_1/max_velocity: 5

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_2/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_2/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_2/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_2/max_velocity: 5

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_3/has_acceleration_limits: False

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_3/has_velocity_limits: True

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_3/max_acceleration: 0

/robot_description_planning/joint_limits/j2n6s300_joint_finger_tip_3/max_velocity: 5

/robot_description_semantic: <?xml version="1....

/robot_type: j2n6s300

/rosdistro: kinetic

/rosversion: 1.12.14

/rviz_gianluca_Precision_5530_12246_8966054062169491260/arm/kinematics_solver: trac_ik_kinematic...

/rviz_gianluca_Precision_5530_12246_8966054062169491260/arm/kinematics_solver_attempts: 3

/rviz_gianluca_Precision_5530_12246_8966054062169491260/arm/kinematics_solver_search_resolution: 0.005

/rviz_gianluca_Precision_5530_12246_8966054062169491260/arm/kinematics_solver_timeout: 0.005

/rviz_gianluca_Precision_5530_12246_8966054062169491260/arm/solve_type: Distance

Nodes / j2n6s300_gripper_command_action_server (kinova_driver/gripper_command_action_server) j2n6s300_joint_trajectory_action_server (kinova_driver/joint_trajectory_action_server) move_group (moveit_ros_move_group/move_group) rviz_gianluca_Precision_5530_12246_8966054062169491260 (rviz/rviz)

ROS_MASTER_URI=http://localhost:11311

process[j2n6s300_joint_trajectory_action_server-1]: started with pid [12288] process[j2n6s300_gripper_command_action_server-2]: started with pid [12289] process[move_group-3]: started with pid [12308] process[rviz_gianluca_Precision_5530_12246_8966054062169491260-4]: started with pid [12338] [ INFO] [1539675092.865809083]: Loading robot model 'j2n6s300'... [ WARN] [1539675092.865858614]: Skipping virtual joint 'world_joint' because its child frame 'root' does not match the URDF frame 'world' [ INFO] [1539675092.865866844]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1539675092.899062378]: rviz version 1.12.16 [ INFO] [1539675092.899095748]: compiled against Qt version 5.5.1 [ INFO] [1539675092.899105793]: compiled against OGRE version 1.9.0 (Ghadamon) [ INFO] [1539675093.000200924]: IK Using joint j2n6s300_link_1 -3.40282e+38 3.40282e+38 [ INFO] [1539675093.000224688]: IK Using joint j2n6s300_link_2 0.820305 5.46288 [ INFO] [1539675093.000234655]: IK Using joint j2n6s300_link_3 0.331613 5.95157 [ INFO] [1539675093.000244105]: IK Using joint j2n6s300_link_4 -3.40282e+38 3.40282e+38 [ INFO] [1539675093.000252145]: IK Using joint j2n6s300_link_5 -3.40282e+38 3.40282e+38 [ INFO] [1539675093.000261850]: IK Using joint j2n6s300_link_6 -3.40282e+38 3.40282e+38 [ INFO] [1539675093.000273950]: Looking in common namespaces for param name: arm/position_only_ik [ INFO] [1539675093.001570618]: Looking in common namespaces for param name: arm/solve_type [ INFO] [1539675093.003371965]: Using solve type Distance [ INFO] [1539675093.118422902]: Joint_trajectory_action_server receive feedback of trajectory state from topic: /trajectory_controller/state [ INFO] [1539675093.151389294]: Gripper_command__action_server receive feedback of trajectory state from topic: /trajectory_controller/state [ INFO] [1539675093.226271989]: Start Follow_Joint_Trajectory_Action server! [ INFO] [1539675093.226311832]: Waiting for an plan execution (goal) from Moveit [ INFO] [1539675093.235003926]: Start Gripper_Command_Trajectory_Action server! [ERROR] [1539675093.235047782]: SpinnerMonitor: single-threaded spinner after multi-threaded one(s).Attempt to spin a callback queue from two spinners, one of them being single-threaded. In the future this will throw an exception! Only allowed for backwards compatibility. [ INFO] [1539675093.269739542]: Stereo is NOT SUPPORTED [ INFO] [1539675093.269798905]: OpenGl version: 4.5 (GLSL 4.5). [ INFO] [1539675093.379315562]: Publishing maintained planning scene on 'monitored_planning_scene' [ INFO] [1539675093.382304891]: MoveGroup debug mode is ON Starting context monitors... [ INFO] [1539675093.382327571]: Starting scene monitor [ INFO] [1539675093.386190268]: Listening to '/planning_scene' [ INFO] [1539675093.386208214]: Starting world geometry monitor [ INFO] [1539675093.390225417]: Listening to '/collision_object' using message notifier with target frame '/world ' [ INFO] [1539675093.394011246]: Listening to '/planning_scene_world' for planning scene world geometry [ INFO] [1539675093.923543304]: Listening to '/attached_collision_object' for attached collision objects Context monitors started. [ INFO] [1539675093.943431170]: Initializing OMPL interface using ROS parameters [ INFO] [1539675093.961866012]: Using planning interface 'OMPL' [ INFO] [1539675093.964040599]: Param 'default_workspace_bounds' was not set. Using default value: 10 [ INFO] [1539675093.964533859]: Param 'start_state_max_bounds_error' was set to 0.1 [ INFO] [1539675093.964998604]: Param 'start_state_max_dt' was not set. Using default value: 0.5 [ INFO] [1539675093.965489084]: Param 'start_state_max_dt' was not set. Using default value: 0.5 [ INFO] [1539675093.965940523]: Param 'jiggle_fraction' was set to 0.05 [ INFO] [1539675093.966348261]: Param 'max_sampling_attempts' was not set. Using default value: 100 [ INFO] [1539675093.966412928]: Using planning request adapter 'Add Time Parameterization' [ INFO] [1539675093.966435410]: Using planning request adapter 'Fix Workspace Bounds' [ INFO] [1539675093.966443817]: Using planning request adapter 'Fix Start State Bounds' [ INFO] [1539675093.966471799]: Using planning request adapter 'Fix Start State In Collision' [ INFO] [1539675093.966495303]: Using planning request adapter 'Fix Start State Path Constraints' [ INFO] [1539675094.197430707]: Added FollowJointTrajectory controller for j2n6s300 [ INFO] [1539675094.459615239]: Added GripperCommand controller for j2n6s300_gripper [ INFO] [1539675094.459775971]: Returned 2 controllers in list [ INFO] [1539675094.499437203]: Trajectory execution is managing controllers Loading 'move_group/ApplyPlanningSceneService'... Loading 'move_group/ClearOctomapService'... Loading 'move_group/MoveGroupCartesianPathService'... Loading 'move_group/MoveGroupExecuteService'... 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] [1539675094.559186979]:

MoveGroup using:

ApplyPlanningSceneService ClearOctomapService CartesianPathService ExecuteTrajectoryService ExecuteTrajectoryAction GetPlanningSceneService KinematicsService MoveAction PickPlaceAction MotionPlanService QueryPlannersService StateValidationService

[ INFO] [1539675094.559238423]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner [ INFO] [1539675094.559262136]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1539675096.600745707]: Loading robot model 'j2n6s300'... [ WARN] [1539675096.600783562]: Skipping virtual joint 'world_joint' because its child frame 'root' does not match the URDF frame 'world' [ INFO] [1539675096.600796192]: No root/virtual joint specified in SRDF. Assuming fixed joint [ INFO] [1539675096.728993287]: IK Using joint j2n6s300_link_1 -3.40282e+38 3.40282e+38 [ INFO] [1539675096.729016979]: IK Using joint j2n6s300_link_2 0.820305 5.46288 [ INFO] [1539675096.729024856]: IK Using joint j2n6s300_link_3 0.331613 5.95157 [ INFO] [1539675096.729034509]: IK Using joint j2n6s300_link_4 -3.40282e+38 3.40282e+38 [ INFO] [1539675096.729043796]: IK Using joint j2n6s300_link_5 -3.40282e+38 3.40282e+38 [ INFO] [1539675096.729054038]: IK Using joint j2n6s300_link_6 -3.40282e+38 3.40282e+38 [ INFO] [1539675096.729065184]: Looking in common namespaces for param name: arm/position_only_ik [ INFO] [1539675096.730448782]: Looking in common namespaces for param name: arm/solve_type [ INFO] [1539675096.731743431]: Using solve type Distance [ INFO] [1539675097.110711393]: Starting scene monitor [ INFO] [1539675097.113231275]: Listening to '/move_group/monitored_planning_scene' [ INFO] [1539675097.303263567]: Constructing new MoveGroup connection for group 'arm' in namespace '' [ WARN] [1539675098.114149640]: Deprecation warning: Trajectory execution service is deprecated (was replaced by an action). Replace 'MoveGroupExecuteService' with 'MoveGroupExecuteTrajectoryAction' in move_group.launch [ INFO] [1539675098.118180083]: Ready to take commands for planning group arm. [ INFO] [1539675098.118243910]: Looking around: no [ INFO] [1539675098.118286444]: Replanning: no [ WARN] [1539675098.147702080]: Interactive marker 'EE:goal_j2n6s300_end_effector' contains unnormalized quaternions. This warning will only be output once but may be true for others; enable DEBUG messages for ros.rviz.quaternions to see more details.

Only this second terminal reports some output after the plan and execute commands, but neither in the simulation nor in the real robot I can see the robot moving after the execute command. By the way, the output are:

[ INFO] [1539675221.547000869]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [ INFO] [1539675221.547545174]: Starting state is just outside bounds (joint 'j2n6s300_joint_3'). Assuming within bounds. [ INFO] [1539675221.550612073]: Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [ INFO] [1539675221.552412395]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.552523881]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.552594350]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.552700737]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.563649081]: RRTConnect: Created 4 states (2 start + 2 goal) [ INFO] [1539675221.563772152]: RRTConnect: Created 4 states (2 start + 2 goal) [ INFO] [1539675221.564075898]: RRTConnect: Created 5 states (3 start + 2 goal) [ INFO] [1539675221.564102790]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.564343745]: ParallelPlan::solve(): Solution found by one or more threads in 0.012663 seconds [ INFO] [1539675221.564496216]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.564522638]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.564624888]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.564708631]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.565510815]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.565770428]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.566061960]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.566183781]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.566285197]: ParallelPlan::solve(): Solution found by one or more threads in 0.001830 seconds [ INFO] [1539675221.566371800]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.566423286]: RRTConnect: Starting planning with 1 states already in datastructure [ INFO] [1539675221.567235312]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.567258788]: RRTConnect: Created 5 states (2 start + 3 goal) [ INFO] [1539675221.567394125]: ParallelPlan::solve(): Solution found by one or more threads in 0.001052 seconds [ INFO] [1539675221.570944656]: SimpleSetup: Path simplification took 0.003489 seconds and changed from 3 to 2 states [ INFO] [1539675221.571582488]: Planning adapters have added states at index positions: [ 0 ] [ INFO] [1539675225.505017409]: Received new trajectory execution service request... [ INFO] [1539675225.548000573]: Joint_trajectory_action_server received goal! [ INFO] [1539675225.548230797]: Joint_trajectory_action_server accepted goal! [ INFO] [1539675225.548400106]: Joint_trajectory_action_server published goal via command publisher! [ INFO] [1539675234.346898869]: Completed trajectory execution with status SUCCEEDED ... [ INFO] [1539675234.347204269]: Execution completed: SUCCEEDED

I really don't know what to do. Thank you very much

martine1406 commented 6 years ago

Hi @GianlucaCerilli

The output from the first terminal indicates that the connection with the real robot is successful. Can you send a printscreen of what you are seeing in Rviz when launching roslaunch j2n6s300_moveit_config j2n6s300_demo.launch?

As Nassif said, we officially support ROS Indigo. The two exact same steps you do (roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2n6s300, then roslaunch j2n6s300_moveit_config j2n6s300_demo.launch) work fine on my Ubuntu 14.04 computer with ROS indigo.

Thank you Martine

GianlucaCerilli commented 6 years ago

Hello Martine,

I did not know that you supported only Indigo...since you are more experienced maybe you can find the problem, but I don't want to bother you. By the way, this is the screen when Rviz opens:

And this is the other screen when I update the goal pose before doing execute:

Another possible help. When rviz is open and I move the robot from terminal with command rosrun kinova_demo pose_action_client.py -v -r j2n6s300 mdeg -- 0.1 0 0 0 0 0 I can see the robot moving and updating its pose in Rviz. But when I do execute in Rviz the robot (simulator and real) does not move...

Thank you very much

martine1406 commented 6 years ago

Hi @GianlucaCerilli

Is you real robot initially in the same position (home position) as shown in the first picture?

GianlucaCerilli commented 6 years ago

Hi Martine, Yes, I put it there with rosservice call /j2n6s300_driver/in/home_arm. In fact, the grey pose in the second image keeps updated with the movements of the real robot. But the contrary does not happen. As you can see in the second terminal it tells me that I have unnormalized quaternion. Indeed, If I use rostopic echo /j2n6s300_driver/out/tool_pose I get: I think this is the pose of the end effector wrt to the base (p.s. how can I get the poses of the other links, included the link_base?). I don't know if it is normal to have it in this format, but I don't think this is related to the fact that the robot does not move...

martine1406 commented 6 years ago

Hi @GianlucaCerilli

Is the planning phase in moveit working (when you click plan)?

when you monitor /j2n6s300_driver/out/tool_pose, you get the pose of the end effector with respect to the base as you say. If you want the other frames in ROS, you have to monitor the tf transforms (with tf echo). The output you get seems appropriate (anything you feel is wrong?)

The behaviour you get in Kinetic is the same as what we get in Indigo, but the execution phase seems not to work. If the real robot is not moving, the grey model of the robot will not move either.

I'll have to test on a Ubuntu 16.04/ROS Kinetic computer here at Kinova. I'll let you know when I have time. Let me know if you have progression on your end

GianlucaCerilli commented 6 years ago

Hi @martine1406

Yes, when I click plan I can see the grey robot moving to the desired position. It is only the execute command that does not working in both systems (even if I get successful results on the terminal). Thank you very much Martine1406 for your availability. Let me know when you get some results.

If you want the other frames in ROS, you have to monitor the tf transforms (with tf echo). The output you get seems appropriate (anything you feel is wrong?)

In the output from /j2n6s300_driver/out/tool_pose (and also if I look at the other link poses in rviz) I have the quaternions that are not normalized, so I was wondering if this is something that I should correct. In addition to this, If I use rostopic echo /tf for the _link_base - link1 frames, I get:

where the translation on Z axis is about 0.15675, while the "base to shoulder" D1 distance should be 0.2755.

So I am not sure if the parameters that I get from echo /tf and _/out/toolpose are correct or if it is necessary to correct something (e.g. normalizing, ...). I don't really understand also this other result on the _end_effector pose wrt the linkbase, in which the orientation from /out/tool_pose, seems to be quite weird wrt to the one that I get from rviz and from my script.

Particularly, from Rviz, I get:

From rostopic /j2n6s300_driver/out/tool_pose I get:

position: x: 0.214647 y: -0.257467 z: 0.508646 orientation: x: 0.64426 y: 0.315656 z: 0.426122 w: 0.551099

And from this script:

string target_frame = "j2n6s300_link_base"; string source_frame = "j2n6s300_end_effector";

tf::StampedTransform tmp_tf; Eigen::Affine3d T, T_inv; Matrix< double, 3, 4 > transformation_frames; Matrix3d rotation_from_transformation; Vector3d translation_from_transformation; Eigen::Quaterniond q_vec; Vector4d quaternion_from_transformation;

tflistener.lookupTransform(target_frame, source_frame, ros::Time(0), tmp_tf); tf2Affine(tmp_tf, T);

for(int i=0; i<T.rows(); i++){ for(int j=0; j<T.cols(); j++){ transformation_frames(i,j) = T(i,j); }}

rotation_from_transformation = transformation_frames.block<3,3>(0,0); translation_from_transformation = transformation_frames.col(3); Eigen::Vector3d t_vec = translation_from_transformation; q_vec = Eigen::Quaterniond(rotation_from_transformation); q_vec.normalize(); quaternion_from_transformation(0) = q_vec.x(); quaternion_from_transformation(1) = q_vec.y(); quaternion_from_transformation(2) = q_vec.z(); quaternion_from_transformation(3) = q_vec.w();

I get:

Translation: 0.21461 -0.257426 0.508693

Rotation: 0.678763 -0.232358 0.691 0.0883721

Sorry again for the long message and thank you very much for your time and kindness.

Sincerely

martine1406 commented 6 years ago

Hi @GianlucaCerilli

In the output from /j2n6s300_driver/out/tool_pose (and also if I look at the other link poses in rviz) I have the quaternions that are not normalized, so I was wondering if this is something that I should correct.

I guess you can normalize the quaternion if desired

If I use rostopic echo /tf for the link_base - link_1 frames, I get (...) where the translation on Z axis is about 0.15675, while the "base to shoulder" D1 distance should be 0.2755.

The frames in Rviz are located at each joints. The frames defined for the real robot are DH frames following the classical convention. The intermediate frames are simply not the same in Rviz and in the real robot, hence the difference you observe.

I don't really understand also this other result on the end_effector pose wrt the link_base, in which the orientation from /out/tool_pose, seems to be quite weird wrt to the one that I get from rviz and from my script...

Please check the frames again. I think the end effector frame in Rviz is not exactly the same as on the real robot (I remember seeing a 90 rotation offset between the two frames at one point). The frames for the real robot are found at p. 85 of the robot's user guide https://www.kinovarobotics.com/sites/default/files/ULWS-RA-JAC-UG-INT-EN%20201804-1.0%20%28KINOVA%E2%84%A2%20Ultra%20lightweight%20robotic%20arm%20user%20guide%29_0.pdf.

I'll check if we could correct this last error on the master branch

Thank you Martine

GianlucaCerilli commented 6 years ago

Hi @martine1406 ,

Ok, now I understand. I would like to mount a camera on the end effector, calibrate it and derive its frame wrt to the link_base frame (and end-effector frame), so I think I need the frame of the real robot. Is it possible to get them somehow in ros or only the rviz frames are available?

Thank you Gianluca

martine1406 commented 6 years ago

Hi @GianlucaCerilli

You can certainly have the same end effector frame in Rviz and on the real robot (that would actually make more sense, I'll check if we can correct that in the package). If you want to correct in on your end, you just have to modify the urdf file and change the joint_end_effector_origin_rpy. I do not specifically think that you need to have equivalent intermediate frame between the real robot and Rviz, but let me know if you think otherwise.

Martine

GianlucaCerilli commented 6 years ago

Hi @martine1406 ,

I think it could be better to have the possibility of getting all the real frames' poses for real applications and not (or not only, at least) the Rviz frame poses. I also wanted to ask two additional informations if possible. Is there any repository for this j2n6s300 for calibrating a camera mounted on the end effector? And, since the robot is mounted on the provided kit, a translation on Z axis for the base frame (as world frame) should be taken in consideration. Is it reported somewhere the precise measure of this translation?

Thank you again and sorry for the many questions. Let me know when you will get some result or correction.

Gianluca

martine1406 commented 6 years ago

Hi @GianlucaCerilli

Rviz need the frames positioned at the joints. The robot does not have all the intermediate frames positioned at the joints. What you can do is perhaps define additional tf frames that will fit the real robot's frames and allow you to use tf echo to track those frames' motion without impairing the display in Rviz?

For you two additional questions:

Is there any repository for this j2n6s300 for calibrating a camera mounted on the end effector? no, no ready-to-use repo for this kind of process :-( Although we might be able to advise if you have specific questions.
And, since the robot is mounted on the provided kit, a translation on Z axis for the base frame (as world frame) should be taken in consideration. Is it reported somewhere the precise measure of this translation? no. The reason for that is that many people decide to mount the robot on custom-made mountings made from standard aluminum extrusions instead of the provided kit. Can you measure this translation on your mounting if that's not too complicated?

Thank you

GianlucaCerilli commented 6 years ago

Ok, @martine1406 , thank you very much for your (many!) replies. You have been very kind.

I will let you know for eventual further doubts in the next days, otherwise let me know please if you get some results from the execute problem in Rviz or if you correct the package in order to get the end-effector frame in real coordinates.

Thank you again and have a nice day. Gianluca

martine1406 commented 6 years ago

We will correct the package in the next few days. I have someone working on it. I am be able to test with a robot on Ubuntu 16.04 at the end of the day. I have some priorities I have to handle first, but thanks for the reminder :)

martine1406 commented 6 years ago

HI @GianlucaCerilli

The urdf is corrected. However, we tested on Ubuntu 16.04 and could not reproduce you isue with the execute.

We proceeded the following way

clone the package on the Ubuntu 16.04
checkout to commit 94c25e1
roslaunch kinova_bringup kinova_robot.launch kinova_robotType:=j2n6s300
roslaunch j2n6s300_moveit_config j2n6s300_demo.launch

I am not sure how we can help more :-O

martine1406 commented 6 years ago

Hi again

Finally, the person who did the test had tested on the wrong computer. When testing on Ubuntu 16.04, we also have a problem at the execution. However, in my case, I see the goal being aborted, not succeeded, in the moveit terminal. According to the error message, it seems that moveit has trouble accessible the robot's joint_state within 1 sec or something. As this package is not officially supported on Ubuntu 16.04, I cannot really spend much time right now on the investigation, but still, I'll let you know if I have something. Sorry about that. Martine

GianlucaCerilli commented 6 years ago

Hi @martine1406 ,

Thank you very much! Let me know If you will figure out the problem.

Thank you also for the urdf correction. I will update the moveit.rviz file according to your changes. Can you please tell me what are the values of the real end-effector frame that you get, for example at home position? So, I can compare with mine to understand if the values that I get are the correct ones.

Thank you again! Gianluca

martine1406 commented 6 years ago

from rostopic echo /j2n6s300_driver/out/tool_pose pose: position: x: 0.212557017803 y: -0.256574690342 z: 0.508452177048 orientation: x: 0.643426954746 y: 0.320419073105 z: 0.422684729099 w: 0.55197006464

from rosrun tf tf_echo /j2n6s300_link_base /j2n6s300_end_effector

Translation: [0.213, -0.257, 0.508]
Rotation: in Quaternion [0.643, 0.320, 0.423, 0.552] in RPY (radian) [1.605, -0.191, 1.109] in RPY (degree) [91.947, -10.970, 63.546]

GianlucaCerilli commented 6 years ago

Thank you @martine1406,

The problem is that I am on the commit 94c25e1 (from branch kinova-ros-beta) that works on kinetic but you did the last commit with the changes for the urdf on the branch master, that does not compile on kinetic. Is it possible to update that changes also from my position or from branch kinova-ros-beta?

martine1406 commented 6 years ago

just replace the needed urdf files in your repo after you switched to commit 94c25e1? The changes for this bug are in all urdf files (robot_code).xacro (e.g. j2n6s300.xacro)

GianlucaCerilli commented 6 years ago

Thank you very much @martine1406 , I have this result now, a bit different from yours, but I definitively think is the correct one:

rostopic echo /j2n6s300_driver/out/tool_pose

pose: position: x: 0.213109344244 y: -0.256380349398 z: 0.509006023407 orientation: x: 0.642293870449 y: 0.319399952888 z: 0.425574630499 w: 0.551659822464

rosrun tf tf_echo /j2n6s300_link_base /j2n6s300_end_effector

Translation: [0.213, -0.256, 0.509]

Rotation: in Quaternion [0.642, 0.319, 0.426, 0.552] in RPY (radian) [1.600, -0.196, 1.113] in RPY (degree) [91.701, -11.203, 63.758]

Thank you again! Gianluca

martine1406 commented 6 years ago

great! Yes it might depend slightly on the encoders' readings.

woshisj commented 5 years ago

just replace the needed urdf files in your repo after you switched to commit 94c25e1? The changes for this bug are in all urdf files (robot_code).xacro (e.g. j2n6s300.xacro)

Hi I checkout to commit 94c25e1 in ubuntu16 and the real robot not movement in rviz This problem solved？

g1y5x3 commented 5 years ago

I am having the same issue. I was looking at the topic connection from rqt_graph and noticed that there is a topic /m1n6s200_driver/trajectory_controller/command is not subscribed by the m1n6s200_driver node. rosgraph

@martine1406 Can you possibly show me the rqt_graph on your working version? I just want to see the exact connection through different topics between all the nodes.

Thanks so much!

g1y5x3 commented 5 years ago

I am having the same issue. I was looking at the topic connection from rqt_graph and noticed that there is a topic /m1n6s200_driver/trajectory_controller/command is not subscribed by the m1n6s200_driver node.

@martine1406 Can you possibly show me the rqt_graph on your working version? I just want to see the exact connection through different topics between all the nodes.

Thanks so much!

I have found a solution. Here is the link to my answer! mico cannot move using moveit #234

felixmaisonneuve commented 3 years ago

This thread has being inactive for over 2 years, I am closing this issue. If there is still a problem, please create a new issue.

Thank You

Kinovarobotics / kinova-ros

No movement on real robot #172