[mycoboteus] [MyCobot] euslisp interface for mycobot 280

.travis.rosinstall.kinetic

@@ -28,6 +28,12 @@
     local-name: fetchrobotics/fetch_ros
     uri: https://github.com/fetchrobotics/fetch_ros.git
     version: 0ad250a996a2ef68172fc064698f253bc9cf08d0
+# jks_mycobot_startup requires mycobot_ros with following PR
+# https://github.com/elephantrobotics/mycobot_ros/pull/48
+- git:
+    local-name: mycobot_ros
+    uri: https://github.com/Michi-Tsubaki/mycobot_ros.git
+    version: mycobot280
 - git:
     local-name: visualization_rwt/rwt_app_chooser
     uri: https://github.com/tork-a/visualization_rwt-release.git

.travis.rosinstall.melodic

@@ -49,6 +49,12 @@
 - git:
     local-name: UbiquityRobotics/magni_robot
     uri: https://github.com/UbiquityRobotics/magni_robot
+# jks_mycobot_startup requires mycobot_ros with following PR
+# https://github.com/elephantrobotics/mycobot_ros/pull/48
+- git:
+    local-name: mycobot_ros
+    uri: https://github.com/Michi-Tsubaki/mycobot_ros.git
+    version: mycobot280
 # denso_cobotta_control, denso_cobotta_descriptions, denso_cobotta_driver, and denso_cobotta_gripper are not released
 - git:
     local-name: DENSORobot/denso_cobotta_ros

jsk_mycobot_robot/README.md

@@ -0,0 +1,112 @@
+# jsk_mycobot_robot
+
+ROS package for Mycobot ARM robot.
+
+- Mycobot web page
+  https://www.elephantrobotics.com/en/mycobot-en/
+- GitHub
+  https://github.com/elephantrobotics/mycobot_ros
+
+## How to setup the Mycobot robot
+
+under construction 
+
+## How to setup development environment
+
+### My Setup
+
+We need `pymycobot` for conection with M5Stack for the arm. This modules is supported in both python2 and python3
+
+```bash
+pip install pymycobot
+```
+
+### ROS Environment Setup
+
+Use `wstool`, `rosdep` and `catkin` to checkout and compile the source tree.
+
+```bash
+mkdir -p ~/mycobot_ws/src
+cd ~/mycobot_ws/src
+wstool init
+wstool merge https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_mycobot_robot/mycobot.rosinstall
+wstool update
+cd ../
+source /opt/ros/melodic/setup.bash
+rosdep install -y -r --from-paths src --ignore-src
+catkin build jsk_mycobot_startup mycoboteus
+source devel/setup.bash
+```
+
+## Start ROS Node
+
+```bash
+# Mycobot280 without gripper
+roslaunch jsk_mycobot_startup mycobot_bringup.launch # rviz:=true for visualize
+```
+
+## Use EusLisp model
+To control the robot form EusLisp. Please start `roseus` and type as follows.
+```
+
+(load "package://mycoboteus/mycobot-interface.l")
+;; Mycobot280 without gripper
+(mycobot-init)
+```
+
+Use `:angle-vector` method to specify the arm joint angle (the max is 100 degree)
+```
+(send *robot* :angle-vector #f(90.0 15.0 0.0 0.0 0.0 55.0 0.0)) 
+```
+
+You can also use `:inverse-kinematics` method to specify the arm pose from target coordinates.
+```
+(send *ri* :angle-vector (send *robot* :rarm :inverse-kinematics (make-coords :pos #f(280 0 100)) :debug-view t) 3000)
+```
+
+To control real robot. you can use *ri* object.
+```
+(send *ri* :angle-vector (send *robot* :angle-vector) 2000)
+```
+2000 indicates we ask robot to move for 2000 [msec]
+
+To obtain current robot pose, use `:state :potentio-vector` method.
+```
+(send *ri* :state :potentio-vector)
+```
+
+## Gazebo simulation
+
+Mycobot robot without gripper.
+```bash
+# Terminal 1
+roslaunch mycobot_280_moveit gazebo.launch
+# Terminal 2
+roscd mycoboteus
+roseus mycobot-interface.l
+(mycobot-init)
+## sample of IK
+(send *robot* :rarm :inverse-kinematics (make-coords :pos #f(280 0 100)))
+(send *ri* :angle-vector (send *robot* :angle-vector))
+```
+
+## Try with moveit
+
+```bash
+# Terminal 1
+roslaunch jsk_mycobot_startup mycobot_bringup.launch moveit:=true # rviz:=true for visualize
+# Or in gazebo:
+roslaunch mycobot_280_moveit demo_gazebo.launch
+# Terminal 2
+roscd mycoboteus
+roseus mycobot-interface.l
+(mycobot-init)
+## try eusIK (goal point) + moveit plan (trajectory)
+(send *ri* :angle-vector-motion-plan #f(45 45 45 45 45 45) :move-arm :rarm :total-time 5000)
+```
+
+
+
+## Trouble shooting
+
+under construction 

jsk_mycobot_robot/jsk_mycobot_startup/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(jsk_mycobot_startup)
+
+# Find catkin macros and libraries
+find_package(catkin)
+
+# catkin specific configuration
+catkin_package()
+
+# Install
+catkin_install_python(PROGRAMS
+  scripts/mycobot_interface.py
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts
+)
+
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+  USE_SOURCE_PERMISSIONS)
+
+# Test
+if(CATKIN_ENABLE_TESTING)
+  find_package(catkin REQUIRED COMPONENTS roslaunch roslint)
+
+  file(GLOB LAUNCH_FILES launch/*.launch)
+  foreach(LAUNCH_FILE ${LAUNCH_FILES})
+    roslaunch_add_file_check(${LAUNCH_FILE})
+  endforeach()
+
+  set(ROSLINT_PYTHON_OPTS --max-line-length=180 --ignore=E221,E222,E241) # skip multiple spaces before/after operator
+  roslint_python()
+  #roslint_add_test()
+endif()

jsk_mycobot_robot/jsk_mycobot_startup/launch/mycobot_bringup.launch

@@ -0,0 +1,34 @@
+<launch>
+  <arg name="port" default="/dev/ttyUSB0" doc ="the serial port for connection" />
+  <arg name="baud" default="115200" doc ="the serial baudrate" />
+  <arg name="rviz" default="true" doc="Set true if you want RViz." />
+  <arg name="moveit" default="false" doc="Set true if you want Moveit-based plan." />
+  <arg name="robot_type" default="mycobot_280_m5"/>
+  <arg name="urdf_path" default="$(find mycobot_description)/urdf/$(arg robot_type)/mycobot_280_m5.urdf"/>
+
+  <!-- Driver -->
+  <node pkg="jsk_mycobot_startup" type="mycobot_interface.py" name="mycobot_bridge"  output="screen" >
+    <param name="port" value="$(arg port)" />
+    <param name="baud" value="$(arg baud)" />
+    <param name="model" value="$(arg robot_type)" />
+  </node>
+
+  <!-- robot state publisher for tf -->
+  <param name="robot_description" textfile="$(arg urdf_path)" />
+  <node name="$(anon robot_state_publisher)" pkg="robot_state_publisher" type="robot_state_publisher" />
+
+
+  <!-- Moveit -->
+  <include if="$(arg moveit)" file="$(find mycobot_280_moveit)/launch/move_group.launch" >
+    <arg name="info" value="true" />
+  </include>
+
+  <!-- Rviz -->
+  <node
+    if="$(arg rviz)"
+    pkg="rviz"
+    type="rviz"
+    name="rviz"
+    args="-d $(find mycobot_280)/config/mycobot.rviz" />
+
+</launch>

jsk_mycobot_robot/jsk_mycobot_startup/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>jsk_mycobot_startup</name>
+  <version>1.1.0</version>
+  <description>
+    jsk_mycobot_startup
+  </description>
+
+  <maintainer email="chou@jsk.imi.i.u-tokyo.ac.jp">Moju Zhao</maintainer>
+  <maintainer email="tsubaki@jsk.t.u-tokyo.ac.jp">Michitoshi Tsubaki</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <exec_depend>mycobot_communication</exec_depend>
+  <exec_depend>mycoboteus</exec_depend>
+  <exec_depend>mycobot_280</exec_depend>
+  <exec_depend>robot_state_publisher</exec_depend>
+  <exec_depend>rviz</exec_depend>
+
+  <test_depend>roslaunch</test_depend>
+  <test_depend>roslint</test_depend>
+  <test_depend>rostest</test_depend>
+
+  <export>
+  </export>
+</package>