drawzeropoint/curi-walker

Team CURI‘s WAIC 2020 Walker competition codebase (3rd place in the final)

CURI-Walker

Team CURI's solution for World Artificial Intelligence Conference (WAIC) 2020 Walker challenge competition. Source Code

Pre-requests

Ubuntu 18.04, ROS Melodic, Webots R2020a

Project structure

ubt_sim_ws

The catkin workspace for development. Two official packages named example and ubt_core_msgs exist in src/. Besides that, we have a series of packages started with walker_ prefix that are tailored for this contest, with the exception of the hope package, which is based on a prior work of our team.

A comprehensive introduction of the customized packages is as follows:

hope: The object detection package that could detect horizontal planes and objects on the plane given the point cloud of the scene. We use this in GraspCup, PushCart, and OpenFridge tasks, which helps for locating the cup to grasp, the cart, or the fridge.

walker_brain: The primary entrance for solving the tasks. Refer its README for details.

walker_description: URDF and meshes for the Walker robot.

walker_moveit_config: MoveIt! configure files generated from walker description.

walker_movement: Primary control functionalists.

walker_nav: Primary navigation stack.

walker_webots_hardware_interface: Interface for interacting with the Webots simulator.

walker_install

Official pre-built gait package.

walker_model

Official Webots model and controllers. No modification should be carried out in this part.

Installation

1. In ~, git clone --recrusive https://github.com/DrawZeroPoint/CURI-Walker.git. You could manually re-create the ubt_sim_ws following this ROS instruction, and then move the contents back.

2. In new terminal, source /opt/ros/molodic/setup.bash, and then source ~/CURI-Walker/walker_install/setup.bash, and finally catkin_make in ubt_sim_ws.

3. Add source ~/CURI-Walker/ubt_sim_ws/devel/setup.bash into ~/.bashrc. After that, echo $ROS_PACKAGE_PATH should give some output like this: /home/x/CURI-Walker/ubt_sim_ws/src:/home/x/CURI-Walker/walker_install/share:/opt/ros/melodic/share

Execution

1. Run roscore

2. Start Webots, open WAIC.wbt in walker_model/worlds/WAIC.wbt

3. Refer README in walker_brain for usage guide.

4. Prepare to record the videos with kazam.

Launching MoveIt

To control the Walker robot via MoveIt you can use moveit.launch from the walker_webots_hardware_interface package. This launcher will start the MoveIt move_group, together with the hardware interface to the simulated robot.

roslaunch walker_webots_hardware_interface moveit.launch show_rviz:=true

This configuration allows to control via MoveIt the arms, the head and the legs of the Walker robot. By default the legs control is disabled to avoid interference with the gait module, you can enable it using enable_legs:=true

NOTE: for this to work you may need to install additional system packages, you can use rosdep to install them automatically. Just move to ubt_sim_ws and run:

rosdep install --from-paths src --ignore-src -r -y

Implementation details

Three different packages have been defined to handle the robot control.

• walker_description provides the urdf and the 3D meshes that represent the robot
• walker_moveit_config defines the MoveIt configuration
• walker_webots_hardware_interface provides the hardware_interface that connects the simulated robot to MoveIt.

Cheat sheet

Webots API

Services

Name	Type	Content
/walker/sence	webots_api/SceneSelection	.scene_name “SwitchLight”, “GraspCup”, “PushCart”, “OpenFridge”, “CarryBox”, “Upstairs” .nav true, false .vision true, false

Task-scene relation

scene name	nav/vision	task
SwitchLight	F/F	1
GraspCup	F/F	2
	F/T	3
	T/F	4
	T/F	5
PushCart	F/F	6
	F/T	7
	T/F	8
	T/F	9
OpenFridge	F/F	10
	F/T	11
	T/F	12
	T/F	13
CarryBox	F/F	14
Upstairs	F/F	15

Topics

Name	Type	Content
/Leg/DesiredJoint	sensor_msgs/JointState
/walker/Leg/MeasuredJoint	sensor_msgs/JointState
/sensor/head_imu	sensor_msgs/Imu
/sensor/orientus_imu	sensor_msgs/Imu
/sensor/camera_imu	sensor_msgs/Imu
/sensor/ft/lankle	geometry_msgs/WrenchStamped
/sensor/ft/lwrist	geometry_msgs/WrenchStamped
/sensor/ft/rankle	geometry_msgs/WrenchStamped
/sensor/ft/rwrist	geometry_msgs/WrenchStamped
/walker/camera/bottomDepth	sensor_msgs/Image
/walker/camera/bottomRGB	sensor_msgs/Image
/walker/camera/doubleLeftRGB	sensor_msgs/Image
/walker/camera/doubleRightRGB	sensor_msgs/Image
/walker/camera/headDepth	sensor_msgs/Image	+z: front, +x: right
/walker/camera/headRGB	sensor_msgs/Image
/walker/camera/topDepth	sensor_msgs/Image
/walker/camera/topRGB	sensor_msgs/Image
/walker/head/controller	ubt_core_msgs/JointCommand	.mode 5 (P), 6 (V), 7 (F) .command.resize(2) [0] HeadYaw, [1] HeadPitch
/walker/head/joint_states	sensor_msgs/JointState
/walker/leftHand/controller	ubt_core_msgs/JointCommand	.mode 5 (P), 6 (V), 7 (F) .command.resize(10) [0] LFirstFinger1, [1] LFirstFinger2, [2] LSecondFinger1, [3] LSecondFinger2, [4] LThirdFinger1, [5] LThirdFinger2, [6] LForthFinger1, [7] LForthFinger2, [8] LFifthFinger1, [9] LFifthFinger2
/walker/leftHand/joint_states	sensor_msgs/JointState
/walker/leftLimb/controller	ubt_core_msgs/JointCommand	.mode 5 (P), 6 (V), 7 (F) .command.resize(7) [0] LShoulderPitch, [1] LShoulderRoll, [2] LShoulderYaw, [3] LElbowRoll, [4] LElbowYaw, [5] LWristPitch, [6] LWristRoll
/walker/leftLimb/joint_states	sensor_msgs/JointState
/walker/rightHand/controller	ubt_core_msgs/JointCommand	.mode 5 (P), 6 (V), 7 (F) .command.resize(10) [0] RFirstFinger1, [1] RFirstFinger2, [2] RSecondFinger1, [3] RSecondFinger2, [4] RThirdFinger1, [5] RThirdFinger2, [6] RForthFinger1, [7] RForthFinger2, [8] RFifthFinger1, [9] RFifthFinger2
/walker/rightHand/joint_states	sensor_msgs/JointState
/walker/rightLimb/controller	ubt_core_msgs/JointCommand	.mode 5 (P), 6 (V), 7 (F) .command.resize(7) [0] RShoulderPitch, [1] RShoulderRoll, [2] RShoulderYaw, [3] RElbowRoll, [4] RElbowYaw, [5] RWristPitch, [6] RWristRoll
/walker/rightLimb/joint_states	sensor_msgs/JointState
/walker/ultrasound/leftBack	sensor_msgs/Range
/walker/ultrasound/leftFront	sensor_msgs/Range
/walker/ultrasound/middleBack	sensor_msgs/Range
/walker/ultrasound/rightBack	sensor_msgs/Range
/walker/ultrasound/rightFront	sensor_msgs/Range

Gait API

Services

Name	Type	Content
/Leg/TaskScheduler	walker_srvs	.func_name “dynamic” .param_json “” .cmd “start”, “stop”

Topics

Name	Type	Content
/ID_status	std_msgs/String
/Leg/BodyPlanner	geometry_msgs/Point
/Leg/CmdToDynamic	std_msgs/Int64
/Leg/DesiredJoint	sensor_msgs/JointState
/Leg/DisLeft2Arm	sensor_msgs/JointState
/Leg/DisRight2Arm	sensor_msgs/JointState
/Leg/FixedPlanning	geometry_msgs/Pose2D
/Leg/Joint2Sim	sensor_msgs/JointState
/Leg/RandomOutput	geometry_msgs/PointStamped
/Leg/StepNum	std_msgs/Int64
/Leg/StopfromScript	std_msgs/Bool
/Leg/WaistPoseDesired	geometry_msgs/PoseStamped
/Leg/WaistPoseMeasured	geometry_msgs/PoseStamped
/Leg/dis_waist2ankle_left	geometry_msgs/Point
/Leg/dis_waist2ankle_right	geometry_msgs/Point
/Leg/footpose2camera	geometry_msgs/PoseStamped
/Leg/footpose2waist	geometry_msgs/PoseStamped
/Leg/left_leg_ground	std_msgs/Bool
/Leg/leg_status	std_msgs/String
/Leg/robot_vel	geometry_msgs/Pose2D
/Leg/walking_odom	nav_msgs/Odometry
/Leg/walking_status	std_msgs/String
/Robot_mode	geometry_msgs/Twist
/astra_aruco/markers	aruco_msgs/MarkerArray
/astra_aruco_marker_publisher_2/markers	aruco_msgs/MarkerArray
/nav/cmd_vel_nav	geometry_msgs/Twist
/tf	tf2_msgs/TFMessage

Issues

1. The Webots simulator could not run in real-time.

   According to the official statement, the dynamic simulation is time consuming where 0.2x real-time is an reasonable performance, so no need to worry about that and the finial videos will be judged basing on the simulation time.

2. Webots crash on startup.

   /usr/local/bin/webots: line 86: 27518 Segmentation fault (core dumped) "$webots_home/bin/webots-bin" "$@"

   Rollback any changes in the file walker_model/worlds/.WAIC.wbproj

curi-walker's Issues

Error when launching the helpers.launch

Hi @c-rizz

When I tried to use the helpers.launch to start the actions, the following error occured:

[ERROR] [1592814610.290985177]: Robot model parameter not found! Did you remap 'robot_description'?
[FATAL] [1592814610.291065850]: Unable to construct robot model. Please make sure all needed information is on the parameter server.

Is this the same namespace issue we used to meet?

CMakeLists issue

Hi @c-rizz
The following error occured when CMaking:

CMake Error at /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:305 (message):
  catkin_package() include dir 'include' does not exist relative to
  '/home/dzp/CURI-Walker/ubt_sim_ws/src/walker_movement'
Call Stack (most recent call first):
  /opt/ros/melodic/share/catkin/cmake/catkin_package.cmake:102 (_catkin_package)
  walker_movement/CMakeLists.txt:105 (catkin_package)

I assume the include folder is empty and we could just remove it from line 106.

walker_nav enhancement

Hi @killazfern
When running walker_nav, I notice a couple of things that could be improved:

1. The user_account.json file could be put into a ROS package, such that we could refer that with $(find xxx) instead of change the path manually like this:

    <include file="$(find leg_motion)/launch/walker2_leg.launch">
      <arg name="account_file" value="/home/dzp/CURI-Walker/user_account.json"/>
    </include>

2. The following script gives 'unused args show_rviz for include of [...]' in my setting despite that show_rviz is defined in moveit.launch. After digging into that I found the param is wrapped by namespace /walker in moveit.launch, and simply moving it out of the ns group could fix this. Would you check out this with @c-rizz for a best solution:

    <include file="$(find walker_webots_hardware_interface)/launch/moveit.launch">
      <arg name="show_rviz" value="false"/>
    </include>

3. It would be great to have a readme about walker_nav, describing the dependencies, which map to visualize in RViz, and what the expected string should be in services like:

# A predefined string would be better, and also do we really need to use String instead of string?
std_msgs/String response

For now, I could only visualize /rtabmap/proj_map, which looks not like the whole map:

Can't launch task 7

@DrawZeroPoint

Launching

roslaunch walker_brain prepare_push_cart.launch nav:=false

and then

roslaunch walker_brain push_cart.launch task_id:=7

gives:

ERROR: cannot launch node of type [walker_brain/open_fridge_node]: Cannot locate node of type [open_fridge_node] in package [walker_brain]. Make sure file exists in package path and permission is set to executable (chmod +x)

Header not found

Hi @DrawZeroPoint
I'm having an issue building on master with catkin_make:

[ 97%] Building CXX object example/CMakeFiles/example_node.dir/src/walker_arm_kin.cpp.o
/home/april/carlo/CURI-Walker/ubt_sim_ws/src/walker_brain/src/grasp_cup_node.cpp:8:10: fatal error: hope/ExtractObjectOnTop.h: No such file or directory
 #include <hope/ExtractObjectOnTop.h>
          ^~~~~~~~~~~~~~~~~~~~~~~~~~~
compilation terminated.
walker_brain/CMakeFiles/grasp_cup_node.dir/build.make:62: recipe for target 'walker_brain/CMakeFiles/grasp_cup_node.dir/src/grasp_cup_node.cpp.o' failed
make[2]: *** [walker_brain/CMakeFiles/grasp_cup_node.dir/src/grasp_cup_node.cpp.o] Error 1
CMakeFiles/Makefile2:2595: recipe for target 'walker_brain/CMakeFiles/grasp_cup_node.dir/all' failed
make[1]: *** [walker_brain/CMakeFiles/grasp_cup_node.dir/all] Error 2
make[1]: *** Waiting for unfinished jobs....
[ 98%] Linking CXX executable /home/april/carlo/CURI-Walker/ubt_sim_ws/devel/lib/walker_webots_hardware_interface/walker_webots_hardware_interface_node
[ 98%] Built target walker_webots_hardware_interface_node
[100%] Linking CXX executable /home/april/carlo/CURI-Walker/ubt_sim_ws/devel/lib/example/example_node
[100%] Built target example_node
Makefile:140: recipe for target 'all' failed
make: *** [all] Error 2

Running walker_brain open_fridge.launch task_id:=12

->roslaunch walker_brain open_fridge.launch task_id:=12
... logging to /home/mfernandes/.ros/log/8a9af478-bb89-11ea-9500-983b8faa3d27/roslaunch-DevBuntu18-18462.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://DevBuntu18:36799/

SUMMARY

PARAMETERS

• /open_fridge_helper/rotation_tolerance: 1
• /open_fridge_helper/x_offset: 0.38
• /open_fridge_helper/y_offset: 0.44
• /open_fridge_node/tree_file: /home/mfernandes/...
• /rosdistro: melodic
• /rosversion: 1.14.5
• /use_sim_time: True

NODES
/
open_fridge_helper (walker_brain/open_fridge_helper.py)
open_fridge_node (walker_brain/open_fridge_node)

ROS_MASTER_URI=http://localhost:11311

process[open_fridge_helper-1]: started with pid [18512]
process[open_fridge_node-2]: started with pid [18513]
terminate called after throwing an instance of 'BT::RuntimeError'
what(): Possible typo? In the XML, you tried to remap port "execute_pose" in node [ExecuteHeadJointStates / ExecuteHeadJointStates], but the manifest of this node does not contain a port with this name.
[open_fridge_node-2] process has died [pid 18513, exit code -6, cmd /home/mfernandes/repos/Simucenas/CURI-Walker/ubt_sim_ws/devel/lib/walker_brain/open_fridge_node __name:=open_fridge_node __log:=/home/mfernandes/.ros/log/8a9af478-bb89-11ea-9500-983b8faa3d27/open_fridge_node-2.log].
log file: /home/mfernandes/.ros/log/8a9af478-bb89-11ea-9500-983b8faa3d27/open_fridge_node-2*.log

Error when using walker_nav

Hi @killazfern
When I run walker_nav gmapping_test, I found that these 2 dependencies should be added to README:

ros-melodic-map-server ros-melodic-amcl

cup task issue

I tried to execute navigation+ cup grasp tasks but i have an error

its the same error for task_id:=4 and task_id:=5
->roslaunch walker_brain grasp_cup.launch task_id:=5 cup_id:=5
... logging to /home/mfernandes/.ros/log/ac1a6c34-baf1-11ea-b949-983b8faa3d27/roslaunch-DevBuntu18-14801.log
Checking log directory for disk usage. This may take a while.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://DevBuntu18:45477/

SUMMARY

PARAMETERS

• /grasp_cup_helper/target_id: 5
• /grasp_cup_helper/x_offset: 0.43
• /grasp_cup_helper/y_offset: 0.28
• /grasp_cup_helper/z_offset: 0.15
• /grasp_cup_node/task_id: 5
• /grasp_cup_node/tree_file: /home/mfernandes/...
• /rosdistro: melodic
• /rosversion: 1.14.5
• /use_sim_time: True

NODES
/
grasp_cup_helper (walker_brain/grasp_cup_helper.py)
grasp_cup_node (walker_brain/grasp_cup_node)

ROS_MASTER_URI=http://localhost:11311

process[grasp_cup_helper-1]: started with pid [14839]
process[grasp_cup_node-2]: started with pid [14840]
terminate called after throwing an instance of 'BT::RuntimeError'
what(): Error parsing the XML: XML_ERROR_FILE_NOT_FOUND
[grasp_cup_node-2] process has died [pid 14840, exit code -6, cmd /home/mfernandes/repos/Simucenas/CURI-Walker/ubt_sim_ws/devel/lib/walker_brain/grasp_cup_node __name:=grasp_cup_node __log:=/home/mfernandes/.ros/log/ac1a6c34-baf1-11ea-b949-983b8faa3d27/grasp_cup_node-2.log].
log file: /home/mfernandes/.ros/log/ac1a6c34-baf1-11ea-b949-983b8faa3d27/grasp_cup_node-2*.log
^C[grasp_cup_helper-1] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete

Controller Spawner couldn't find the expected controller_manager ROS interface.

Hi @c-rizz
When I launch with roslaunch walker_webots_hardware_interface moveit.launch show_rviz:=true in the master branch, it gives the following warnings in the console:

[WARN] [1592098443.430520, 42.430000]: Controller Spawner couldn't find the expected controller_manager ROS interface.
[controller_spawner-1] process has finished cleanly
log file: /home/dzp/.ros/log/914220ac-adde-11ea-a4c7-50e085f74fae/controller_spawner-1*.log
[ WARN] [1592098447.470499250, 43.179000000]: Waiting for walker_head_controller/follow_joint_trajectory to come up
[ERROR] [1592098454.145625153, 44.413000000]: Unable to connect to move_group action server 'move_group' within allotted time (30s)
[ WARN] [1592098479.912189158, 49.179000000]: Waiting for walker_head_controller/follow_joint_trajectory to come up
[ERROR] [1592098512.679894687, 55.179000000]: Action client not connected: walker_head_controller/follow_joint_trajectory
[ WARN] [1592098540.262980096, 60.192000000]: Waiting for walker_left_arm_controller/follow_joint_trajectory to come up
[ WARN] [1592098573.007816716, 66.192000000]: Waiting for walker_left_arm_controller/follow_joint_trajectory to come up
[ERROR] [1592098605.572370569, 72.192000000]: Action client not connected: walker_left_arm_controller/follow_joint_trajectory
[ WARN] [1592098632.661653896, 77.200000000]: Waiting for walker_right_arm_controller/follow_joint_trajectory to come up

As a result, no dynamic tf is published. Here is the ouput tf tree, only static tf is present.

The output of rosservice list | grep controller_manager is:

/walker/controller_manager/list_controller_types
/walker/controller_manager/list_controllers
/walker/controller_manager/load_controller
/walker/controller_manager/reload_controller_libraries
/walker/controller_manager/switch_controller
/walker/controller_manager/unload_controller

And rosservice call /walker/controller_manager/list_controllers gives:

controller: []

Quick googling give me clues for installing missing packages. However, when I run rosdep install --from-paths src --ignore-src -r -y at ~/CURI-Walker/ubt_sim_ws, the output is:

ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
example: Cannot locate rosdep definition for [kdl]
Continuing to install resolvable dependencies...
#All required rosdeps installed successfully

Not sure whether the error matters.

So I wonder how to correctly bring up those controllers, get the tf, and maybe set my ROS packages in a right way.