Forums Niryo One Programming Niryo One Queuing move goals Reply To: Queuing move goals

achernov
Participant
Post count: 7

Alright, we tried the approach with SimpleActionClient you suggested, but unfortunately the result is the same – no movement. Attaching rosout.log for investigation. In the log you can see that topic message is still used, but actually with goal the output is identical.



1549558269.605691035  Node Startup
1549558271.531095981 INFO [niryo_one_ros_setup.py:212(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Start process : robot_commander
1549558271.863245010 INFO [rosbridge_websocket:224(<module>) [topics: /rosout] Rosbridge WebSocket server started on port 9090
1549558272.940612077 INFO [rosapi_node:194(<module>) [topics: /rosout] Rosapi started
1549558275.529162036 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:148(init) [topics: /rosout] 3 motors should be connected to CAN bus
1549558275.542890203 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_driver/niryo_one_can_driver.cpp:50(init) [topics: /rosout] Result begin can : 0
1549558275.549954891 INFO [niryo_one_ros_setup.py:209(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Handle process : user_interface
1549558275.551881074 INFO [niryo_one_ros_setup.py:212(start_process) [topics: /niryo_one/rpi/process_state, /rosout] Start process : user_interface
1549558275.628074136 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:66(init) [topics: /rosout] Start Dxl communication (100.000000 Hz)
1549558275.628778612 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:67(init) [topics: /rosout] Writing data on Dxl at 50.000000 Hz
1549558275.629420849 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:68(init) [topics: /rosout] Reading data from Dxl at 15.000000 Hz
1549558275.630011888 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:69(init) [topics: /rosout] Reading hardware error status from Dxl at 0.500000 Hz
1549558275.638224350 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:183(setupCommunication) [topics: /rosout] Dxl : set port name (/dev/serial0), baudrate(1000000)
1549558275.740333675 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:132(NiryoOneDriver) [topics: /rosout] NiryoOne communication has been successfully started
1549558275.841226079 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:137(NiryoOneDriver) [topics: /rosout] Start hardware control loop
1549558275.842100814 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:107(manageCanConnectionLoop) [topics: /rosout] Stop Can hw control
1549558275.842230762 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:234(stopHardwareControlLoop) [topics: /rosout] CanComm : Stop hardware control loop
1549558276.192944211 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:117(manageCanConnectionLoop) [topics: /rosout] Set calibration flag
1549558276.194608839 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:212(startHardwareControlLoop) [topics: /rosout] CAN : Start hardware control loop
1549558276.194786129 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/can_communication.cpp:227(startHardwareControlLoop) [topics: /rosout] START ctrl loop thread can
1549558276.264389457 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1193(detectVersion) [topics: /rosout] Dynamixel broadcast ping - Found IDs:
1549558276.264680289 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 2
1549558276.264767268 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 3
1549558276.264840913 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1195(detectVersion) [topics: /rosout] - 6
1549558276.285623943 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:209(checkHardwareVersionFromDxlMotors) [topics: /rosout] Detected version from hardware : 2
1549558276.285868734 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:235(manageDxlConnectionLoop) [topics: /rosout] Stop Dxl hw control
1549558276.343995864 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:141(NiryoOneDriver) [topics: /rosout] Start hardware interface
1549558276.344553570 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_hardware_interface.cpp:25(NiryoOneHardwareInterface) [topics: /rosout] Starting NiryoOne Hardware Interface...
1549558276.345264765 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_hardware_interface.cpp:59(NiryoOneHardwareInterface) [topics: /rosout] Interfaces registered.
1549558276.345676742 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:144(NiryoOneDriver) [topics: /rosout] Create controller manager
1549558276.355998992 INFO [spawner:116(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/load_controller
1549558276.587620271 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:148(NiryoOneDriver) [topics: /rosout] Starting ros control thread...
1549558276.588396518 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:152(NiryoOneDriver) [topics: /rosout] Start Rpi Diagnostics...
1549558276.588872609 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:155(NiryoOneDriver) [topics: /rosout] Starting ROS interface...
1549558276.597320904 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/ros_interface.cpp:39(RosInterface) [topics: /rosout] Ros interface started.
1549558276.667877912 INFO [spawner:121(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/switch_controller
1549558276.674745082 INFO [spawner:129(main) [topics: /rosout] Controller Spawner: Waiting for service controller_manager/unload_controller
1549558276.681794881 INFO [spawner:184(main) [topics: /rosout] Loading controller: joint_state_controller
1549558276.729506015 INFO [spawner:184(main) [topics: /rosout] Loading controller: niryo_one_follow_joint_trajectory_controller
1549558276.807495308 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1096(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] Dynamixel broadcast ping - Found IDs:
1549558276.807902285 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 2
1549558276.808345981 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 3
1549558276.808831969 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:1098(scanAndCheck) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] - 6
1549558276.808974780 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/niryo_one_communication.cpp:244(manageDxlConnectionLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] Resume Dxl hw control
1549558276.810454357 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:222(startHardwareControlLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] DXL : Start hardware control loop
1549558276.810941855 WARN [/home/niryo/catkin_ws/src/niryo_one_driver/src/hw_comm/dxl_communication.cpp:235(startHardwareControlLoop) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states] START ctrl loop thread dxl
1549558276.866338968 INFO [tool_ros_command_interface.py:50(__init__) [topics: /rosout] Interface between Tools Controller and Ros Control has been started.
1549558276.914160013 INFO [tool_controller.py:74(create_tools) [topics: /niryo_one/current_tool_id, /rosout] Detected tools : (id + available commands)
1549558276.916560888 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 11 : [1, 2]
1549558276.918932914 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 12 : [1, 2]
1549558276.921161890 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 13 : [1, 2]
1549558276.923299074 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 30 : [21, 22, 20]
1549558276.925488948 INFO [tool_controller.py:76(create_tools) [topics: /niryo_one/current_tool_id, /rosout] 31 : [10, 11]
1549558277.024730920 INFO [tool_controller.py:184(start) [topics: /niryo_one/current_tool_id, /niryo_one/tool_action/goal, /niryo_one/tool_action/cancel, /niryo_one/tool_action/result, /rosout, /niryo_one/tool_action/feedback, /niryo_one/tool_action/status] Action server started (ToolController)
1549558277.079272031 INFO [spawner:192(main) [topics: /rosout] Controller Spawner: Loaded controllers: joint_state_controller, niryo_one_follow_joint_trajectory_controller
1549558277.090075016 INFO [spawner:201(main) [topics: /rosout] Started controllers: joint_state_controller, niryo_one_follow_joint_trajectory_controller
1549558280.560875391 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558280.918588496 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
1549558280.919279378 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558281.083474461 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:331(PlanningSceneMonitor::startPublishingPlanningScene) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Publishing maintained planning scene on 'monitored_planning_scene'
1549558281.107437528 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group.cpp:210(main) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] MoveGroup debug mode is OFF
1549558281.107718673 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:964(PlanningSceneMonitor::startSceneMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Starting scene monitor
1549558281.127478583 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:970(PlanningSceneMonitor::startSceneMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/planning_scene'
1549558281.127767019 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1041(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Starting world geometry monitor
1549558281.148004999 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1057(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/collision_object' using message notifier with target frame '/ground_link '
1549558281.176345963 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1072(PlanningSceneMonitor::startWorldGeometryMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/planning_scene_world' for planning scene world geometry
1549558281.254341805 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_scene_monitor/src/planning_scene_monitor.cpp:1134(PlanningSceneMonitor::startStateMonitor) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Listening to '/attached_collision_object' for attached collision objects
1549558281.584281703 INFO [/tmp/binarydeb/ros-kinetic-moveit-planners-ompl-0.9.15/ompl_interface/src/ompl_interface.cpp:55(OMPLInterface::OMPLInterface) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene] Initializing OMPL interface using ROS parameters
1549558281.815234033 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:119(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning interface 'OMPL'
1549558281.874300377 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_workspace_bounds.cpp:53(FixWorkspaceBounds::FixWorkspaceBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'default_workspace_bounds' was not set. Using default value: 10
1549558281.878841606 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp:60(FixStartStateBounds::FixStartStateBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_bounds_error' was set to 0.3
1549558281.887674951 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_bounds.cpp:65(FixStartStateBounds::FixStartStateBounds) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_dt' was not set. Using default value: 0.5
1549558281.893030552 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:57(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'start_state_max_dt' was not set. Using default value: 0.5
1549558281.897912769 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:68(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'jiggle_fraction' was set to 0.05
1549558281.902076969 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_request_adapter_plugins/src/fix_start_state_collision.cpp:73(FixStartStateCollision::FixStartStateCollision) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Param 'max_sampling_attempts' was not set. Using default value: 100
1549558281.902636810 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Add Time Parameterization'
1549558281.902838945 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Workspace Bounds'
1549558281.902994621 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State Bounds'
1549558281.903144777 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State In Collision'
1549558281.903278422 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/planning_pipeline/src/planning_pipeline.cpp:163(PlanningPipeline::configure) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates] Using planning request adapter 'Fix Start State Path Constraints'
1549558282.291514043 INFO [/tmp/binarydeb/ros-kinetic-moveit-simple-controller-manager-0.9.15/src/moveit_simple_controller_manager.cpp:146(MoveItSimpleControllerManager::MoveItSimpleControllerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel] Added FollowJointTrajectory controller for niryo_one_follow_joint_trajectory_controller
1549558282.292304664 INFO [/tmp/binarydeb/ros-kinetic-moveit-simple-controller-manager-0.9.15/src/moveit_simple_controller_manager.cpp:197(MoveItSimpleControllerManager::getControllersList) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel] Returned 1 controllers in list
1549558282.401289114 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/trajectory_execution_manager/src/trajectory_execution_manager.cpp:188(TrajectoryExecutionManager::initialize) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates] Trajectory execution is managing controllers
1549558283.117929432 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group.cpp:176(MoveGroupExe::configureCapabilities) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] 

********************************************************
* MoveGroup using: 
*     - ApplyPlanningSceneService
*     - ClearOctomapService
*     - CartesianPathService
*     - ExecuteTrajectoryAction
*     - GetPlanningSceneService
*     - KinematicsService
*     - MoveAction
*     - PickPlaceAction
*     - MotionPlanService
*     - QueryPlannersService
*     - StateValidationService
********************************************************

1549558283.118471044 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group_context.cpp:84(MoveGroupContext::status) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] MoveGroup context using planning plugin ompl_interface/OMPLPlanner
1549558283.118763282 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/move_group_context.cpp:85(MoveGroupContext::status) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] MoveGroup context initialization complete
1549558284.178380886 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558284.496585161 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
1549558284.497022919 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558284.629981040 INFO [moveitjoy_module.py:408(updatePlanningGroup) [topics: /tf_static, /rviz/moveit/select_planning_group, /rosout, /tf] Changed planning group to arm
1549558284.633936882 INFO [moveitjoy_module.py:421(updatePoseTopic) [topics: /tf_static, /rviz/moveit/select_planning_group, /rosout, /tf] Changed controlled end effector to tool_link
1549558284.751899957 INFO [user_interface.py:38(__init__) [topics: /rviz/moveit/move_marker/goal_tool_link, /rviz/moveit/execute, /joy_pose, /rviz/moveit/update_start_state, /rosout, /tf, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /tf_static, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /joy, /rviz/moveit/select_planning_group] ===========MoveitJoy loaded==============
1549558284.883557081 INFO [blockly_code_generator.py:66(__init__) [topics: /rviz/moveit/move_marker/goal_tool_link, /rviz/moveit/execute, /joy_pose, /rviz/moveit/update_start_state, /rosout, /tf, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /tf_static, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /joy, /rviz/moveit/select_planning_group] Blockly code generator started
1549558284.787094145 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558285.012191057 INFO [sequence_action_server.py:54(start) [topics: /rviz/moveit/execute, /niryo_one/sequences/execute/cancel, /rosout, /tf, /tf_static, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /niryo_one/sequences/execute/result, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/sequences/execute/feedback, /rviz/moveit/update_start_state, /joy_pose, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /joy, /niryo_one/sequences/execute/status] Action Server started (Sequence Action)
1549558285.219535112 INFO [sequence_autorun.py:215(__init__) [topics: /rviz/moveit/execute, /niryo_one/sequences/execute/cancel, /rosout, /tf, /tf_static, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /niryo_one/sequences/execute/result, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/learning_mode, /niryo_one/sequences/execute/feedback, /rviz/moveit/update_start_state, /joy_pose, /rviz/moveit/update_goal_state, /rviz/moveit/plan, /joy, /niryo_one/hardware_status, /niryo_one/sequences/execute/status, /niryo_one/sequences/sequence_autorun_status] Init Sequence autorun OK
1549558285.233821842 INFO [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:93(core::RobotModel::buildModel) [topics: /rosout] Loading robot model 'niryo_one'...
1549558285.234876577 WARN [/tmp/binarydeb/ros-kinetic-moveit-core-0.9.15/robot_model/src/robot_model.cpp:111(core::RobotModel::buildModel) [topics: /rosout] No geometry is associated to any robot links
1549558285.250288963 INFO [matlab_manager.py:62(__init__) [topics: /rviz/moveit/execute, /rviz/moveit/move_marker/goal_tool_link, /niryo_one/sequences/execute/cancel, /rosout, /niryo_one_matlab/command, /tf, /niryo_one/sequences/execute/goal, /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full, /rviz/moveit/select_planning_group, /rviz/moveit/update_goal_state, /niryo_one/sequences/sequence_autorun_status, /rviz/moveit/plan, /niryo_one/learning_mode, /niryo_one_matlab/result, /rviz/moveit/update_start_state, /joy_pose, /niryo_one/sequences/execute/result, /tf_static, /joy, /niryo_one/hardware_status, /niryo_one/sequences/execute/status, /niryo_one/sequences/execute/feedback] ..............Matlab OK.................
1549558285.570365905 INFO [wifi_connection.py:89(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Starting wifi manager...
1549558286.343882083 INFO [wifi_connection.py:121(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Current ssid : Guest
1549558286.634478929 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-interface-0.9.15/move_group_interface/src/move_group_interface.cpp:180(planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::MoveGroupInterfaceImpl) [topics: /rosout, /trajectory_execution_event, /attached_collision_object, /move_group/goal, /move_group/cancel, /pickup/goal, /pickup/cancel, /place/goal, /place/cancel, /execute_trajectory/goal, /execute_trajectory/cancel] Ready to take commands for planning group arm.
1549558286.635123926 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-interface-0.9.15/move_group_interface/src/move_group_interface.cpp:1085(planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::allowReplanning) [topics: /rosout, /trajectory_execution_event, /attached_collision_object, /move_group/goal, /move_group/cancel, /pickup/goal, /pickup/cancel, /place/goal, /place/cancel, /execute_trajectory/goal, /execute_trajectory/cancel] Replanning: yes
1549558286.665504932 INFO [move_group_arm.py:65(__init__) [topics: /tf_static, /niryo_one/robot_state, /rosout, /tf] Successfully connected to move_group.
Started group     : arm
Planning_frame    : /ground_link
Reference frame   : ground_link
End effector link : tool_link
1549558286.673681974 INFO [move_group_arm.py:67(__init__) [topics: /tf_static, /niryo_one/robot_state, /rosout, /tf] Arm Moveit Commander has been started
1549558287.156985044 INFO [wifi_connection.py:125(__init__) [topics: /niryo_one/rpi/process_state, /rosout] Already connected to a Wifi or in Hotspot mode
1549558287.181430101 INFO [wifi_connection.py:140(__init__) [topics: /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot] Wifi manager started
1549558287.194025993 INFO [tool_commander.py:34(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Waiting for action server : niryo_one/tool_action...
1549558287.233474016 INFO [fans_manager.py:36(setup_fans) [topics: /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot] ------ RPI FANS SETUP OK ------
1549558287.240040063 INFO [tool_commander.py:36(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Found action server : niryo_one/tool_action
1549558287.242249965 INFO [tool_commander.py:38(__init__) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /tf_static, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /tf, /niryo_one/tool_action/status, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/tool_action/feedback] Tool Commander has been started
1549558287.448247909 INFO [robot_commander.py:221(start) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Action Server started (Robot Commander)
1549558287.658555984 INFO [arm_commander.py:106(stop_current_plan) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Send STOP to arm
1549558287.660652310 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/trajectory_execution_manager/src/trajectory_execution_manager.cpp:243(TrajectoryExecutionManager::receiveEvent) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Received event 'stop'
1549558290.841027021 INFO [websocket_handler.py:77(open) [topics: /rosout] Client connected.  1 clients total.
1549558292.287158012 INFO [ros_log_manager.py:147(__init__) [topics: /niryo_one/rpi/process_state, /niryo_one/learning_mode, /rosout, /niryo_one/rpi/ros_log_status, /niryo_one/wifi/hotspot] Init Ros Log Manager OK
1549558292.295151948 INFO [shutdown_manager.py:49(__init__) [topics: /niryo_one/rpi/process_state, /niryo_one/learning_mode, /rosout, /niryo_one/rpi/ros_log_status, /niryo_one/wifi/hotspot] Shutdown Manager OK
1549558292.438963890 INFO [led_manager.py:169(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] LED manager has been started.
1549558292.449472904 INFO [niryo_one_button.py:41(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] GPIO setup : 4 as input
1549558292.553144931 INFO [niryo_one_button.py:54(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Niryo One Button started
1549558292.693665027 INFO [motor_debug.py:32(__init__) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /niryo_one/rpi/digital_io_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Init motor debug OK
1549558292.715090036 INFO [modbus_server.py:57(start) [topics: /niryo_one/learning_mode, /niryo_one/rpi/ros_log_status, /niryo_one/rpi/process_state, /niryo_one/rpi/digital_io_state, /rosout, /niryo_one/wifi/hotspot, /niryo_one/hardware_status] Start Modbus Server
1549558322.431951045 INFO [robot_commander.py:249(callback_trajectory_command) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Robot Action Server - Trajectory command x: 0.225
y: -0.22
z: 0.1 roll: 0.0
pitch: 1.55
yaw: 1.63
1549558322.499579904 INFO [/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/default_capabilities/move_action_capability.cpp:163(MoveGroupMoveAction::executeMoveCallback_PlanOnly) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Planning request received for MoveGroup action. Forwarding to planning pipeline.
1549558322.507166067 INFO [/tmp/binarydeb/ros-kinetic-moveit-planners-ompl-0.9.15/ompl_interface/src/model_based_planning_context.cpp:335(ModelBasedPlanningContext::useConfig) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Planner configuration 'arm' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
1549558322.510276418 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:185(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] RRTConnect: Starting planning with 1 states already in datastructure
1549558322.522835579 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:292(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] RRTConnect: Created 5 states (2 start + 3 goal)
1549558322.522967453 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/src/SimpleSetup.cpp:143(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] Solution found in 0.013272 seconds
1549558322.533046470 INFO [/tmp/binarydeb/ros-kinetic-ompl-1.2.3/src/ompl/geometric/src/SimpleSetup.cpp:185(OMPLPlannerManager::OMPLPlannerManager) [topics: /rosout, /move_group/planning_scene_monitor/parameter_descriptions, /move_group/planning_scene_monitor/parameter_updates, /move_group/monitored_planning_scene, /move_group/ompl/parameter_descriptions, /move_group/ompl/parameter_updates, /move_group/display_planned_path, /move_group/display_contacts, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /move_group/trajectory_execution/parameter_descriptions, /move_group/trajectory_execution/parameter_updates, /move_group/plan_execution/parameter_descriptions, /move_group/plan_execution/parameter_updates, /move_group/sense_for_plan/parameter_descriptions, /move_group/sense_for_plan/parameter_updates, /execute_trajectory/result, /execute_trajectory/feedback, /execute_trajectory/status, /move_group/result, /move_group/feedback, /move_group/status, /pickup/result, /pickup/feedback, /pickup/status, /place/result, /place/feedback, /place/status] SimpleSetup: Path simplification took 0.009394 seconds and changed from 4 to 2 states
1549558322.548414004 INFO [/home/niryo/catkin_ws/src/niryo_one_driver/src/niryo_one_driver_node.cpp:180(callbackTrajectoryGoal) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states, /niryo_one_follow_joint_trajectory_controller/state, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status] Received trajectory GOAL
1549558322.552408933 INFO [robot_commander.py:260(callback_trajectory_command) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Robot Action Server - Trajectory plan is ready
1549558322.585071086 WARN [arm_commander.py:94(send_trajectory_direct) [topics: /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/goal, /niryo_one/steppers_reset_controller, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one/tool_action/result, /rosout, /niryo_one/learning_mode, /tf, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one/commander/robot_action/cancel, /niryo_one/tool_action/feedback, /niryo_one/tool_action/cancel, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/cancel, /niryo_one/commander/robot_action/feedback, /niryo_one/commander/trajectory, /niryo_one/robot_state, /niryo_one/tool_action/goal, /niryo_one/max_velocity_scaling_factor, /niryo_one/joystick_interface/is_enabled, /niryo_one/tool_action/status, /tf_static, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status, /niryo_one/commander/robot_action/result, /niryo_one/commander/robot_action/status, /niryo_one/commander/robot_action/goal, /niryo_one_follow_joint_trajectory_controller/command, /niryo_one/hardware_status] Joint Trajectory Publisher msg: header: 
  seq: 0
  stamp: 
    secs: 1549558322
    nsecs: 553577899
  frame_id: ''
joint_names: [joint_1, joint_2, joint_3, joint_4, joint_5, joint_6]
points: 
  - 
    positions: [-0.3433080631757919, -0.37415659874332574, 0.28124734232137194, 0.0767944870877505, 0.01689478715930511, 0.010122909661567111]
    velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    accelerations: [-0.17109186245792865, 0.0, 0.0, 0.0, 0.0, 0.0]
    effort: []
    time_from_start: 
      secs: 0
      nsecs:         0
  - 
    positions: [-0.35601449063967366, -0.3927600931001226, 0.27286370069659466, 0.07526152967251763, -0.0005283144017771274, -0.061506224863800286]
    velocities: [-0.05647721686756443, -0.08268835502897148, -0.03726340448787656, -0.0068136514874022665, -0.07744177410746675, -0.3183754187496277]
    accelerations: [-0.17276868326339648, -0.25295081825776583, -0.113991972061556, -0.020843548265747486, -0.23690107417124695, -0.9739378979985842]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 385400303
  - 
    positions: [-0.3687209181035554, -0.4113635874569195, 0.2644800590718173, 0.07372857225728478, -0.017951415962859364, -0.13313535938916768]
    velocities: [-0.09210891200311652, -0.1348567588751023, -0.06077303088356602, -0.011112410633560763, -0.12630008970442894, -0.5192397050713495]
    accelerations: [-0.17272710970222613, -0.2528899503613526, -0.11396454201963516, -0.02083853265462873, -0.23684406834638802, -0.9737035380091625]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 544260421
  - 
    positions: [-0.3814273455674372, -0.4299670818137164, 0.25609641744704004, 0.07219561484205192, -0.03537451752394161, -0.2047644939145351]
    velocities: [-0.11319366145181567, -0.16572696361977413, -0.07468465030834745, -0.013656164423332666, -0.1552115781679558, -0.6380994206749324]
    accelerations: [-0.15865263132101015, -0.23228349115918584, -0.10467826677515192, -0.019140527762117006, -0.21754509017552795, -0.8943623771511894]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 666164709
  - 
    positions: [-0.3941337730313189, -0.44857057617051327, 0.24771277582226273, 0.07066265742681906, -0.05279761908502384, -0.2763936284399025]
    velocities: [-0.13010673795543767, -0.19048941743974662, -0.08584381936525659, -0.015696629858194356, -0.17840285285713078, -0.7334424299951676]
    accelerations: [-0.16224449471014624, -0.23754234227843132, -0.10704816150009812, -0.019573865427838982, -0.2224702668863049, -0.9146105599411883]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 770184020
  - 
    positions: [-0.4068402004952007, -0.46717407052731014, 0.23932913419748542, 0.06912970001158619, -0.07022072064610607, -0.34802276296526996]
    velocities: [-0.14582695964797388, -0.21350541122525363, -0.09621594837688455, -0.017593184218668775, -0.19995848050230008, -0.8220610348377976]
    accelerations: [-0.1777947179256261, -0.26030944110764603, -0.11730812630881818, -0.021449910449498396, -0.2437927919745864, -1.0022708431929517]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 862220252
  - 
    positions: [-0.41954662795908243, -0.48577756488410706, 0.2309454925727081, 0.06759674259635334, -0.08764382220718832, -0.41965189749063736]
    velocities: [-0.15915443012155453, -0.2330181753322696, -0.10500935128520816, -0.019201066902204047, -0.21823315859522335, -0.8971911355800024]
    accelerations: [-0.13910070708675726, -0.2036574974886079, -0.09177799828341077, -0.016781700521166797, -0.1907354174633074, -0.7841435595341105]
    effort: []
    time_from_start: 
      secs: 0
      nsecs: 944947110
  - 
    positions: [-0.4322530554229642, -0.5043810592409039, 0.2225618509479308, 0.06606378518112048, -0.10506692376827054, -0.49128103201600465]
    velocities: [-0.16974196315576812, -0.24851939403550194, -0.11199495623999528, -0.020478391887527925, -0.23275082407285422, -0.9568755614467658]
    accelerations: [-0.13421906273648548, -0.1965102766525497, -0.08855711209108486, -0.01619275819835032, -0.1840416882037496, -0.756624576652197]
    effort: []
    time_from_start: 
      secs: 1
      nsecs:  22089516
  - 
    positions: [-0.44495948288684595, -0.5229845535977008, 0.2141782093231535, 0.06453082776588762, -0.12249002532935278, -0.562910166541372]
    velocities: [-0.17926461436820373, -0.2624615180979779, -0.11827795713145317, -0.02162724500381672, -0.24580831955509283, -1.0105569968205241]
    accelerations: [-0.1267415652745897, -0.18556246443458776, -0.08362349411318505, -0.015290641123015954, -0.17378851530584713, -0.7144721563015299]
    effort: []
    time_from_start: 
      secs: 1
      nsecs:  94793220
  - 
    positions: [-0.45766591035072773, -0.5415880479544977, 0.20579456769837617, 0.06299787035065477, -0.139913126890435, -0.6345393010667395]
    velocities: [-0.18855811914983556, -0.2760681486203172, -0.12440976827575961, -0.022748452920754822, -0.25855160858177095, -1.0629466796096363]
    accelerations: [-0.14233526023773266, -0.20839321029712307, -0.09391214137839667, -0.01717193076108833, -0.1951706490195083, -0.8023775000685422]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 163940378
  - 
    positions: [-0.4703723378146095, -0.5601915423112945, 0.1974109260735989, 0.0614649129354219, -0.15733622845151723, -0.706168435592107]
    velocities: [-0.19728255910603715, -0.2888416107089154, -0.1301661131002661, -0.023801006438463196, -0.27051459376014286, -1.1121284094906456]
    accelerations: [-0.12184904369444477, -0.17839931823436772, -0.08039543117528439, -0.014700386524961856, -0.1670798711474179, -0.6868918559041631]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 229655226
  - 
    positions: [-0.48307876527849125, -0.5787950366680914, 0.18902728444882158, 0.05993195552018904, -0.1747593300125995, -0.7777975701174743]
    velocities: [-0.20529302210880382, -0.30056973835855727, -0.13545137925825063, -0.02476742274190056, -0.2814985710303256, -1.157285282550888]
    accelerations: [-0.13194505403890225, -0.19318089802957705, -0.08705673175000386, -0.01591841212387653, -0.18092355884742942, -0.7438054522071391]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 292805904
  - 
    positions: [-0.495785192742373, -0.5973985310248884, 0.18064364282404427, 0.05839899810495618, -0.19218243157368173, -0.8494267046428418]
    velocities: [-0.2125501304759147, -0.31119487866160156, -0.14023958553849497, -0.025642951140117363, -0.2914495455650136, -1.1981953174895474]
    accelerations: [-0.10610334469689399, -0.15534602309862378, -0.07000649235651761, -0.0128007584741229, -0.1454893089248905, -0.5981296294720048]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 353492464
  - 
    positions: [-0.5084916202062547, -0.6160020253816851, 0.172260001199267, 0.05686604068972333, -0.20960553313476393, -0.921055839168209]
    velocities: [-0.21899061197946654, -0.3206243946799271, -0.14448898521989728, -0.02641995820261408, -0.3002807582451793, -1.2345018338047984]
    accelerations: [-0.11263083844193637, -0.16490293383502805, -0.07431330230935328, -0.013588263063256212, -0.15443983312078174, -0.6349266543560576]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 412394241
  - 
    positions: [-0.5211980476701366, -0.6346055197384821, 0.16387635957448965, 0.05533308327449047, -0.22702863469584617, -0.9926849736935763]
    velocities: [-0.22562624651305602, -0.330339634463123, -0.14886714596104517, -0.02722050935611145, -0.30937956550076656, -1.2719084738709177]
    accelerations: [-0.11955990458512829, -0.17504778715896163, -0.0788850678590457, -0.014424215053313429, -0.1639409949131628, -0.6739874377526204]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 469563725
  - 
    positions: [-0.5339044751340183, -0.6532090140952789, 0.15549271794971237, 0.053800125859257614, -0.24445173625692843, -1.0643141082189438]
    velocities: [-0.23246294740864692, -0.34034925572698993, -0.15337796935081044, -0.02804531801011103, -0.3187540757150053, -1.3104485725376804]
    accelerations: [-0.12691524792109024, -0.18581675338751316, -0.08373808911393643, -0.015311594935697755, -0.17402666960989932, -0.7154512464263463]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 525051863
  - 
    positions: [-0.5466109025979, -0.6718125084520759, 0.14710907632493506, 0.05226716844402475, -0.2618748378180107, -1.1359432427443112]
    velocities: [-0.2383034344072293, -0.3489003191339101, -0.15723149545399495, -0.028749939185378305, -0.32676257365289785, -1.3433727780316542]
    accelerations: [-0.08892980239052335, -0.13020222101179377, -0.058675469176814106, -0.010728869337762582, -0.12194088253848114, -0.5013183128658224]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 578908104
  - 
    positions: [-0.5593173300617817, -0.6904160028088727, 0.13872543470015775, 0.05073421102879189, -0.2792979393790929, -1.2075723772696787]
    velocities: [-0.23830343440723034, -0.34890031913390906, -0.15723149545399495, -0.02874993918537824, -0.3267625736528975, -1.3433727780316542]
    accelerations: [0.08892980239048431, 0.13020222101183335, 0.058675469176814106, 0.010728869337765056, 0.12194088253849154, 0.5013183128658224]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 631703164
  - 
    positions: [-0.5720237575256636, -0.7090194971656696, 0.13034179307538044, 0.04920125361355903, -0.29672104094017515, -1.2792015117950462]
    velocities: [-0.23246294740864798, -0.3403492557269908, -0.1533779693508107, -0.02804531801011096, -0.3187540757150048, -1.3104485725376804]
    accelerations: [0.12691524792112832, 0.18581675338740147, 0.0837380891139273, 0.015311594935695344, 0.1740266696098983, 0.7154512464263463]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 685559404
  - 
    positions: [-0.5847301849895453, -0.7276229915224666, 0.12195815145060313, 0.047668296198326174, -0.3141441425012574, -1.3508306463204136]
    velocities: [-0.22562624651305585, -0.3303396344631228, -0.14886714596104506, -0.02722050935611149, -0.30937956550076606, -1.2719084738709177]
    accelerations: [0.11955990458513459, 0.1750477871591103, 0.07888506785906732, 0.014424215053312, 0.16394099491316266, 0.6739874377526198]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 741047542
  - 
    positions: [-0.5974366124534272, -0.7462264858792633, 0.11357450982582581, 0.04613533878309331, -0.3315672440623396, -1.422459780845781]
    velocities: [-0.21899061197946634, -0.32062439467992676, -0.14448898521989717, -0.02641995820261412, -0.30028075824517975, -1.2345018338047984]
    accelerations: [0.11263083844193007, 0.16490293383488733, 0.07431330230933314, 0.013588263063257578, 0.1544398331207491, 0.6349266543560571]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 798217027
  - 
    positions: [-0.6101430399173089, -0.7648299802360603, 0.1051908682010485, 0.044602381367860454, -0.3489903456234219, -1.4940889153711483]
    velocities: [-0.2125501304759147, -0.31119487866160256, -0.14023958553849497, -0.025642951140117422, -0.29144954556501435, -1.1981953174895474]
    accelerations: [0.10610334469689399, 0.15534602309871753, 0.07000649235653246, 0.012800758474120984, 0.1454893089249137, 0.5981296294720048]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 857118804
  - 
    positions: [-0.6228494673811906, -0.7834334745928572, 0.09680722657627122, 0.04306942395262759, -0.36641344718450414, -1.5657180498965158]
    velocities: [-0.20529302210880368, -0.3005697383585561, -0.13545137925825032, -0.024767422741900545, -0.2814985710303252, -1.157285282550888]
    accelerations: [0.13194505403890708, 0.1931808980295545, 0.08705673174999976, 0.015918412123880718, 0.1809235588474427, 0.7438054522071386]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 917805364
  - 
    positions: [-0.6355558948450724, -0.802036968949654, 0.08842358495149391, 0.041536466537394734, -0.38383654874558637, -1.6373471844218832]
    velocities: [-0.19728255910603654, -0.2888416107089152, -0.1301661131002662, -0.02380100643846312, -0.2705145937601424, -1.1121284094906456]
    accelerations: [0.12184904369445328, 0.17839931823436067, 0.08039543117527484, 0.014700386524959582, 0.16707987114740483, 0.6868918559041626]
    effort: []
    time_from_start: 
      secs: 1
      nsecs: 980956041
  - 
    positions: [-0.6482623223089541, -0.8206404633064509, 0.0800399433267166, 0.04000350912216187, -0.4012596503066686, -1.7089763189472507]
    velocities: [-0.1886647987419579, -0.2762243383279022, -0.12448015497348199, -0.022761323200167626, -0.2586978880433527, -1.0635480576820304]
    accelerations: [0.13925395300377505, 0.20388186500349373, 0.09187910922530709, 0.016800188759938613, 0.19094554884636003, 0.7850074429851691]
    effort: []
    time_from_start: 
      secs: 2
      nsecs:  46670889
  - 
    positions: [-0.6609687497728359, -0.8392439576632478, 0.07165630170193929, 0.038470551706929014, -0.41868275186775084, -1.7806054534726181]
    velocities: [-0.17950808246220562, -0.2628179800008774, -0.11843859624523884, -0.021656618029486943, -0.2461421639295826, -1.0119294840052548]
    accelerations: [0.1260647870437986, 0.18457159268611859, 0.0831769589904781, 0.015208991720748972, 0.17286051442729244, 0.7106569974713725]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 115737855
  - 
    positions: [-0.6736751772367175, -0.8578474520200446, 0.06327266007716201, 0.03693759429169616, -0.436105853428833, -1.8522345879979851]
    velocities: [-0.17018895394262218, -0.2491738337357579, -0.11228987867804618, -0.020532318755894022, -0.23336373953617906, -0.9593953541496469]
    accelerations: [0.12993949560915438, 0.19024455773756815, 0.08573347364460124, 0.015676453030715163, 0.17817352951724433, 0.7324996453649099]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 188327930
  - 
    positions: [-0.6863816047005994, -0.8764509463768415, 0.0548890184523847, 0.0354046368764633, -0.45352895498991525, -1.9238637225233526]
    velocities: [-0.1586410663440077, -0.23226655886369518, -0.10467063625721647, -0.019139132514136202, -0.2175292322351649, -0.8942971826426445]
    accelerations: [0.1668343386901454, 0.24426233787304624, 0.11007651916800863, 0.020127603713750403, 0.22876387837076353, 0.9404845951748545]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 265180865
  - 
    positions: [-0.6990880321644812, -0.8950544407336384, 0.046505376827607386, 0.033871679461230444, -0.4709520565509975, -1.9954928570487198]
    velocities: [-0.15054843451787073, -0.22041812774982739, -0.09933115549250535, -0.018162803014606314, -0.2064325847625366, -0.8486771045061041]
    accelerations: [0.03315747124016553, 0.04854588993288419, 0.021877145000153753, 0.004000258259249331, 0.04546565040157303, 0.18691650148965938]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 348804492
  - 
    positions: [-0.711794459628363, -0.9136579350904352, 0.038121735202830076, 0.03233872204599758, -0.4883751581120797, -2.0671219915740875]
    velocities: [-0.14203093324807642, -0.2079476448184898, -0.09371134784889229, -0.01713521545957559, -0.19475335469632832, -0.8006619369062224]
    accelerations: [0.15873130251038364, 0.23239867373364984, 0.104730173659267, 0.019150018988778664, 0.2176529644089601, 0.8948058652380679]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 433997306
  - 
    positions: [-0.7245008870922447, -0.9322614294472321, 0.029738093578052793, 0.030805764630764725, -0.5057982596731619, -2.138751126099455]
    velocities: [-0.12697088039128213, -0.18589820494793483, -0.08377479515849552, -0.015318306673353401, -0.17410295306415802, -0.7157648598084454]
    accelerations: [0.1581009291831485, 0.2314757434553038, 0.10431425627568354, 0.01907396807131059, 0.21678859411030837, 0.8912523018162128]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 528179816
  - 
    positions: [-0.7372073145561264, -0.9508649238040291, 0.021354451953275455, 0.02927280721553186, -0.5232213612342441, -2.2103802606248224]
    velocities: [-0.10981038428388298, -0.16077350381527616, -0.07245237980006641, -0.013247991485887192, -0.15057241567529317, -0.6190270877090148]
    accelerations: [0.15821279854780865, 0.23163953151456076, 0.10438806716115012, 0.019087464466941526, 0.21694198980767637, 0.8918829358629018]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 634930550
  - 
    positions: [-0.7499137420200082, -0.969468418160826, 0.012970810328498172, 0.027739849800299005, -0.5406444627953264, -2.28200939515019]
    velocities: [-0.08758949428180496, -0.12823987444291893, -0.057791140132932704, -0.010567168870829893, -0.12010304697316646, -0.4937626793018072]
    accelerations: [0.17530826265194444, 0.2566690192200563, 0.11566757470688871, 0.021149933917128404, 0.24038335506687486, 0.9882541071909164]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 761247255
  - 
    positions: [-0.7626201694838899, -0.9880719125176228, 0.004587168703720834, 0.02620689238506614, -0.5580675643564086, -2.3536385296755573]
    velocities: [-0.05380014310566368, -0.07876884840416946, -0.03549708369576823, -0.006490677930429152, -0.0737709604052919, -0.3032841212806425]
    accelerations: [0.14974974575155586, 0.21924876665291956, 0.0988041843666298, 0.018066445807180236, 0.20533741969503455, 0.8441747071761887]
    effort: []
    time_from_start: 
      secs: 2
      nsecs: 931604006
  - 
    positions: [-0.7753265969477717, -1.0066754068744197, -0.0037964729210564754, 0.024673934969833285, -0.5754906659174909, -2.4252676642009248]
    velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
    accelerations: [0.1715456256673109, 0.25116013829272216, 0.1131850043594841, 0.020695993399007337, 0.23522401295382364, 0.9670432332840968]
    effort: []
    time_from_start: 
      secs: 3
      nsecs: 316494251
1549558322.616762493 WARN [/tmp/binarydeb/ros-kinetic-joint-trajectory-controller-0.13.4/include/joint_trajectory_controller/init_joint_trajectory.h:324(initJointTrajectory) [topics: /rosout, /niryo_one/hardware_status, /niryo_one/software_version, /niryo_one/learning_mode, /joint_states, /niryo_one_follow_joint_trajectory_controller/state, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/result, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/feedback, /niryo_one_follow_joint_trajectory_controller/follow_joint_trajectory/status] Dropping first 1 trajectory point(s) out of 35, as they occur before the current time.
First valid point will be reached in 0.321s.