diff --git a/jsk_kinova_robot/README.md b/jsk_kinova_robot/README.md new file mode 100644 index 0000000000..0dc0c7d6dc --- /dev/null +++ b/jsk_kinova_robot/README.md @@ -0,0 +1,211 @@ +# jsk_kinova_robot + +ROS package for KINOVA Gen3/Gen3 Lite robot. + +- KINOVA web page + https://www.kinovarobotics.com/en/products/gen3-robot +- GitHub + https://github.com/Kinovarobotics/ros_kortex + +## How to setup the network of KINOVA robot + +This is an overview of the network setup process. +Refer to the documentation on [KINOVA resouce page](https://www.kinovarobotics.com/en/resources/technical-resources-library) if necessary. + +Kinova can be connected with a computer via **USB, Ethernet and Wi-Fi**. + +### Via USB + +Micro-B USB to USB type-A cable is required. + +1. Connect one end of the micro-B USB to type-A cable to the micro-B USB connector in the robot and connect the other end to the computer. +2. DHCP server on the robot base has worked correctly, and then the computer automatically is assigned an IP address. Try to connect to the robot via _Web App_ below. If not, you have to configure the computer RNDIS settings manually. See the steps below. +3. Open the network settings in your computer and set the IPv4 address:192.168.1.11 and Subnet mask:255.255.255.0. + +### Via Ethernet +USB type-A to Ethernet adapter is required. + +1. Connect an Ethernet cable to the robot by using the USB type-A to Ethernet adapter. +2. On your computer, open the network settings and set the IPv4 address:192.168.2.11 and Subnet mask:255.255.255.0. +3. Try to connect to the robot via _Web App_ below. + +### Via Wi-Fi +Connecting your computer to ```133.11.xx.xxx``` is necessary. + +1. Before you begin, you need to have a wired connection between the computer and the robot. **See the chapter Ethernet or USB and complete it** and see _Open Web App_ below. +2. Open Web App and connect to the robot. +3. Go to the Wireless & Networks page under the Configurations page. +4. You can see all of the detected Wi-Fi networks. Choose the networks you use, and fill in the required information. + +### Open Web App +You can interact with the arm and perform basic tasks through an Web browser. + +1. Confirm connecting the robot with a computer. From the web browser, enter the appropriate IP address for the arm base to access the Web App. +- 192.168.1.10 if connecting via USB +- 192.168.2.10 if connecting via Ethernet +- See the robot base if connecting via Wi-Fi. You can read xx.xx.xx...jp here. +2. Fill in the credentials in the login window and click CONNECT. + +## How to setup development environment + +### Conan Setup + +You need to install `conan` (Decentralized, open-source (MIT), C/C++ package manager) to build kinova packages + +```bash +sudo apt install python3 python3-pip +python3 -m pip install --user conan +conan config set general.revisions_enabled=1 +conan profile new default --detect > /dev/null +conan profile update settings.compiler.libcxx=libstdc++11 default +``` + +### ROS Environment Setup + +Use `wstool`, `rosdep` and `catkin` to checkout and compile the source tree. + +```bash +mkdir -p ~/kinova_ws/src +cd ~/kinova_ws/src +wstool init +wstool merge https://raw.githubusercontent.com/jsk-ros-pkg/jsk_robot/master/jsk_kinova_robot/kinova.rosinstall +wstool update +cd ../ +source /opt/ros/melodic/setup.bash +rosdep install -y -r --from-paths src --ignore-src +catkin build jsk_kinova_startup kinovaeus +source devel/setup.bash +``` + +## Start ROS Node + +Start real kinova robot. `ip_address` can be given by both ip address and FQDN (e.g. abc.def.jp) +Use the appropriate ip address according to the type of network you are using +The IP address is the same as when using Web App + +```bash +# Gen3 robot +roslaunch jsk_kinova_startup kinova_bringup.launch ip_address:=xxxx +# Gen3 lite robot +roslaunch jsk_kinova_startup kinova_bringup.launch ip_address:=xxxx arm:=gen3_lite +``` + +Start rviz from different terminal + +```bash +roslaunch jsk_kinova_startup rviz.launch +``` + +## Use EusLisp model +To control the robot form EusLisp. Please start `roseus` and type as follows. +``` + +(load "package://kinovaeus/kinova-interface.l") +;; Gen3 with robotiq 2f 85 gripper +(kinova-init :type :gen3_robotiq_2f_85) +;; Gen3 with robotiq 2f 140 gripper +(kinova-init :type :gen3_robotiq_2f_140) +;; Gen3 Lite +(kinova-init :type :gen3_lite_gen3_lite_2f) +``` + +Use `:angle-vector` method to specify the arm joint angle. +``` +(send *kinova* :angle-vector #f(0.0 15.0 180.0 -130.0 0.0 55.0 90.0)) +``` + +You can also use `:inverse-kinematics` method to specify the arm pose from target coordinates. +``` +(send *ri* :angle-vector (send *kinova* :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector 0 pi/2 0)) :debug-view t) 3000) +``` + +To move the gripper 50 [mm] up, you can use `move-end-pos` method. +``` +(send *kinova* :arm :move-end-pos #f(0 0 -50)) +``` + +You can also use move-end-rot method to turn the gripper. +``` +(send *kinova* :arm :move-end-rot -90 :z) +``` + +To open/close gripper, you can use `start-grasp` and `stop-grasp` method. +``` +(send *ri* :start-grasp) +(send *ri* :stop-grasp) +``` + +To control real robot. you can use *ri* object. +``` +(send *ri* :angle-vector (send *kinova* :angle-vector) 2000) +``` +2000 indicates we ask robot to move for 2000 [msec] + +To obtain current robot pose, use :state :potentio-vector method. +``` +(send *kinova* :arm :move-end-pos #f(0 0 -50)) +``` + +To obtain current robot pose, use `:state :potentio-vector` method. + +``` +(send *ri* :state :potentio-vector) +``` + +To open and close the gripper, You can use `:start-grasp` and `:stop-grasp`. + +``` +(send *ri* :stop-grasp) +(send *ri* :start-grasp) +``` + +## Gazebo simulation +Kinova Gen3 robot with robotiq 2f 85 gripper. +```bash +# Terminal 1 +roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3 gripper:=robotiq_2f_85 robot_name:=arm_gen3 +# Terminal 2 +roscd kinovaeus +roseus kinova-interface.l +(kinova-init :type :gen3_robotiq_2f_85) +``` + +Kinova Gen3 Lite robot with lite 2f gripper. +```bash +# Terminal 1 +roslaunch kortex_gazebo spawn_kortex_robot.launch arm:=gen3_lite gripper:=gen3_lite_2f robot_name:=arm_gen3 +# Terminal 2 +roscd kinovaeus +roseus kinova-interface.l +(kinova-init :type :gen3_lite_gen3_lite_2f) +``` + +## Trouble shooting +- The robot doesn't work without errors in roseus and you see the following error in the terminal where you launched the launch file. TODO + - Turn off the power + - Make a posture that does not exceed the upper or lower limit of the joint angle + - Turn it back on +```bash +----------------------------- +Error #38 +Type : TRAJECTORY_ERROR_TYPE_JOINT_POSITION_LIMIT +Identifier : TRAJECTORY_ERROR_IDENTIFIER_POSITION +Actuator : 3 +Erroneous value is -150.149 but minimum permitted is -150.115 and maximum permitted is 150.115 +Additional message is : Invalid Position - Position of actuator(2) in Trajectory Point (37) exceeds limits +----------------------------- +``` +This error message above also can be seen in the topic `/arm_gen3/gen3_lite_joint_trajectory_controller/follow_joint_trajectory/result`. +- Joint limits are different from the ones in the URDF! This causes the problem above. + - See User guides(ex.[PDF:Gen3_Lite](https://artifactory.kinovaapps.com/artifactory/generic-documentation-public/Documentation/Gen3%20lite/Technical%20documentation/User%20Guide/Gen3_lite_USER_GUIDE_R03.pdf) pp.71-72) + - See URDF descriptions([gen3_7dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/7dof/urdf/gen3_macro.xacro), [gen3_6dof](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3/6dof/urdf/gen3_macro.xacro), [gen3_lite](https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/arms/gen3_lite/6dof/urdf/gen3_lite_macro.xacro)) + +- Error in the initial movement in roseus (under investigation) + - When you used the robot by Web App and then tried to use it by roseus, it occured. +```bash +[ERROR] [1623300893.102460026]: Trajectory has been aborted. +[ERROR] [1623300893.102550198]: Trajectory execution failed in the arm with sub error code 69 +The starting point for the trajectory did not match the actual commanded joint angles. +``` +--- +If you have any question, please feel free to file open at https://github.com/jsk-ros-pkg/jsk_robot/issues diff --git a/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt new file mode 100644 index 0000000000..c0e3d84fbd --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/CMakeLists.txt @@ -0,0 +1,37 @@ +cmake_minimum_required(VERSION 2.8.3) +project(jsk_kinova_startup) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin) + +################################### +## catkin specific configuration ## +################################### +catkin_package() + +############# +## Install ## +############# +install(DIRECTORY config launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +############# +## Testing ## +############# +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS roslaunch roslint) + + file(GLOB LAUNCH_FILES launch/*.launch) + # TODO: Do not do roslaunch_add_file_check() + # because upstream package (kortex_driver and kinova_vision) tests do not pass + # 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() diff --git a/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz b/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz new file mode 100644 index 0000000000..44b72db5fd --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/config/kinova.rviz @@ -0,0 +1,365 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 344 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Image +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + bracelet_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + half_arm_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_finger_pad: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_inner_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_outer_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_finger_pad: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_inner_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_outer_finger: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_outer_knuckle: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + robotiq_arg2f_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spherical_wrist_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + bracelet_link: + Value: true + camera_color_frame: + Value: true + camera_depth_frame: + Value: true + camera_link: + Value: true + end_effector_link: + Value: true + forearm_link: + Value: true + half_arm_1_link: + Value: true + half_arm_2_link: + Value: true + left_inner_finger: + Value: true + left_inner_finger_pad: + Value: true + left_inner_knuckle: + Value: true + left_outer_finger: + Value: true + left_outer_knuckle: + Value: true + right_inner_finger: + Value: true + right_inner_finger_pad: + Value: true + right_inner_knuckle: + Value: true + right_outer_finger: + Value: true + right_outer_knuckle: + Value: true + robotiq_arg2f_base_link: + Value: true + shoulder_link: + Value: true + spherical_wrist_1_link: + Value: true + spherical_wrist_2_link: + Value: true + tool_frame: + Value: true + Marker Scale: 0.4000000059604645 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + base_link: + shoulder_link: + half_arm_1_link: + half_arm_2_link: + forearm_link: + spherical_wrist_1_link: + spherical_wrist_2_link: + bracelet_link: + end_effector_link: + camera_color_frame: + {} + camera_depth_frame: + {} + camera_link: + {} + robotiq_arg2f_base_link: + left_inner_knuckle: + {} + left_outer_knuckle: + left_outer_finger: + left_inner_finger: + left_inner_finger_pad: + {} + right_inner_knuckle: + {} + right_outer_knuckle: + right_outer_finger: + right_inner_finger: + right_inner_finger_pad: + {} + tool_frame: + {} + Update Interval: 0 + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /camera/color/image_rect_color + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: compressed + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 3.203127384185791 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.08768113702535629 + Y: -0.21367596089839935 + Z: 0.5479295253753662 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38539838790893555 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.2504054307937622 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001f3000002b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001e3000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650100000226000000c70000001600fffffffb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670200000532000000c2000001f50000018a000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002b7000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 67 + Y: 27 diff --git a/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch new file mode 100644 index 0000000000..4e0e3b7d3a --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/launch/kinova_bringup.launch @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch b/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch new file mode 100644 index 0000000000..394aad4719 --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/launch/rviz.launch @@ -0,0 +1,5 @@ + + + diff --git a/jsk_kinova_robot/jsk_kinova_startup/package.xml b/jsk_kinova_robot/jsk_kinova_startup/package.xml new file mode 100644 index 0000000000..d26f46b192 --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/package.xml @@ -0,0 +1,31 @@ + + jsk_kinova_startup + 1.1.0 + + jsk_kinova_startup + + Kei Okada + BSD + http://ros.org/wiki/jsk_kinova_startup + catkin + + kinova_teleop + + jsk_robot_startup + kortex_driver + kinova_vision + kinovaeus + robot_state_publisher + rviz + + + diagnostic_aggregator + + roslaunch + roslint + rostest + + + + + diff --git a/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py b/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py new file mode 100644 index 0000000000..d9d50cc66f --- /dev/null +++ b/jsk_kinova_robot/jsk_kinova_startup/scripts/avoid_lint_error.py @@ -0,0 +1,13 @@ +# This file is to avoid roslint_add_test() failure in CMakeLists.txt. +# TODO: Please remove this file after other python files are created. + +# roslint_add_test() failure: +# +# +# +# +# +# diff --git a/jsk_kinova_robot/kinova.rosinstall b/jsk_kinova_robot/kinova.rosinstall new file mode 100644 index 0000000000..fa98c00106 --- /dev/null +++ b/jsk_kinova_robot/kinova.rosinstall @@ -0,0 +1,11 @@ +- git: + local-name: jsk_robot + uri: https://github.com/jsk-ros-pkg/jsk_robot.git + version: master +- git: + local-name: ros_kortex + uri: https://github.com/Kinovarobotics/ros_kortex + version: kinetic-devel +- git: + local-name: ros_kortex_vision + uri: https://github.com/Kinovarobotics/ros_kortex_vision diff --git a/jsk_kinova_robot/kinovaeus/.gitignore b/jsk_kinova_robot/kinovaeus/.gitignore new file mode 100644 index 0000000000..4b6ad1b0d7 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/.gitignore @@ -0,0 +1,3 @@ +gen3_lite_gen3_lite_2f.l +gen3_robotiq_2f_140.l +gen3_robotiq_2f_85.l diff --git a/jsk_kinova_robot/kinovaeus/CMakeLists.txt b/jsk_kinova_robot/kinovaeus/CMakeLists.txt new file mode 100644 index 0000000000..83c5b329cc --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/CMakeLists.txt @@ -0,0 +1,65 @@ +cmake_minimum_required(VERSION 2.8.3) +project(kinovaeus) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + roseus + euscollada +) +find_package(kortex_description) # Just in case when description is not released. Avoid compile failing + +catkin_package() + +if(NOT kortex_description_FOUND) + message(WARNING "kortex_description is not found, so skip generating eus models.") + message(WARNING "Install kortex_description from https://github.com/Kinovarobotics/ros_kortex.git") + return() +endif() + + +########### +## Build ## +########### +if(EXISTS ${kortex_description_SOURCE_PREFIX}/robots) + set(_kinova_urdf ${kortex_description_SOURCE_PREFIX}/robots) +else() + set(_kinova_urdf ${kortex_description_PREFIX}/share/kortex_description/robots) +endif() +set(_collada2eus ${euscollada_PREFIX}/lib/euscollada/collada2eus) + +message("kinova_urdf: ${_kinova_urdf}") +message("collada2eus: ${_collada2eus}") + +# Gen3 robot with robotiq gripper +# Gen3 Lite robot +# xacro command: https://github.com/Kinovarobotics/ros_kortex/tree/kinetic-devel/kortex_description +foreach(ROBOT_TYPE gen3_lite_gen3_lite_2f gen3_robotiq_2f_85 gen3_robotiq_2f_140) + add_custom_command(OUTPUT ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf + COMMAND xacro ${_kinova_urdf}/${ROBOT_TYPE}.xacro sim:=false > ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf + DEPENDS ${_kinova_urdf}/${ROBOT_TYPE}.xacro) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l + COMMAND echo "${_collada2eus} ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${ROBOT_TYPE}.l" + COMMAND ${_collada2eus} ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.yaml ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l + DEPENDS ${PROJECT_BINARY_DIR}/${ROBOT_TYPE}.urdf ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.yaml ${_collada2eus}) + add_custom_target(compile_${ROBOT_TYPE} ALL DEPENDS ${PROJECT_SOURCE_DIR}/${ROBOT_TYPE}.l) +endforeach() + + +install(DIRECTORY test + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +install(FILES kinova.l kinova-interface.l kinova-util.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + find_package(catkin REQUIRED COMPONENTS rostest) + # Test only roseus + add_rostest(test/test-kinova.test) + # Test with Kinematic Simulator + add_rostest(test/test-kinova-kinematic-simulator.test) + # Test with Gazebo and MoveIt! + # TODO: We can use this test after https://github.com/Kinovarobotics/ros_kortex/pull/141 is merged + # add_rostest(test/test-kinova-moveit.test) +endif() diff --git a/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml new file mode 100644 index 0000000000..dee7590b69 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_lite_gen3_lite_2f.yaml @@ -0,0 +1,22 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - joint_6 : rarm-wrist-p + +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_lite_gen3_lite_2f_move_it_config/config/gen3_lite_gen3_lite_2f.srdf.xacro#L31-L38 +angle-vector: + init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + reset-pose: [0.0, -16.0, 75.0, 0.0, -60.0, 0.0] + +rarm-end-coords: + parent : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml new file mode 100644 index 0000000000..d06767b91f --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_140.yaml @@ -0,0 +1,23 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - joint_6 : rarm-wrist-p + - joint_7 : rarm-wrist-y + +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_robotiq_2f_140_move_it_config/config/7dof/gen3_robotiq_2f_140.srdf.xacro#L38-L46 +angle-vector: + init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + reset-pose : [0.0, 15.0, 180.0, -130.0, 0.0, 55.0, 90.0] + +rarm-end-coords: + parent : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] diff --git a/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml new file mode 100644 index 0000000000..2c4b9ad92d --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/gen3_robotiq_2f_85.yaml @@ -0,0 +1,23 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +rarm: + - joint_1 : rarm-collar-y + - joint_2 : rarm-shoulder-p + - joint_3 : rarm-shoulder-y + - joint_4 : rarm-elbow-p + - joint_5 : rarm-elbow-y + - joint_6 : rarm-wrist-p + - joint_7 : rarm-wrist-y + +# reset-pose is the home pose +# https://github.com/Kinovarobotics/ros_kortex/blob/330c55bce8c3d463cca2492b3e0c89204f235640/kortex_move_it_config/gen3_robotiq_2f_85_move_it_config/config/7dof/gen3_robotiq_2f_85.srdf.xacro#L38-L46 +angle-vector: + init-pose : [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + reset-pose : [0.0, 15.0, 180.0, -130.0, 0.0, 55.0, 90.0] + +rarm-end-coords: + parent : tool_frame + translate : [0, 0, 0] + rotate : [-0.57735, -0.57735, -0.57735, 120] diff --git a/jsk_kinova_robot/kinovaeus/kinova-interface.l b/jsk_kinova_robot/kinovaeus/kinova-interface.l new file mode 100644 index 0000000000..75167362c9 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova-interface.l @@ -0,0 +1,997 @@ +(require "package://kinovaeus/kinova-utils.l") +(require "package://pr2eus/robot-interface.l") +(require "package://pr2eus_moveit/euslisp/robot-moveit.l") +(ros::load-ros-manifest "kortex_driver") + +(if (not (find-package "kinova")) + (make-package "kinova")) + +(defclass kinova-interface + :super robot-interface + :slots (controller-handle robot-handle move-robot) + ) + +(defmethod kinova-interface + (:init + (&key type &rest args) + (let () + (prog1 + (send-super* :init + :robot + ;; Create robot instance different from *kinova* + (case type + (:gen3_robotiq_2f_85 + (instance gen3_robotiq_2f_85-robot :init)) + (:gen3_robotiq_2f_140 + (instance gen3_robotiq_2f_140-robot :init)) + (:gen3_lite_gen3_lite_2f + (instance gen3_lite_gen3_lite_2f-robot :init)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" type))) + :joint-states-topic "/arm_gen3/joint_states" + :groupname "kinova_interface" args) + (send self :add-controller :arm-controller) + (setq gripper-action + (instance ros::simple-action-client :init + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + "/arm_gen3/robotiq_2f_140_gripper_controller/gripper_cmd") + ((derivedp robot gen3_robotiq_2f_85-robot) + "/arm_gen3/robotiq_2f_85_gripper_controller/gripper_cmd") + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + "/arm_gen3/gen3_lite_2f_gripper_controller/gripper_cmd") + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot) + )) + control_msgs::GripperCommandAction + :groupname groupname)) + (setq moveit-robot (instance (eval (send (class robot) :name)) :init)) + (send self :set-moveit-environment (instance kinova-moveit-environment :init :robot moveit-robot)) + (ros::advertise "/arm_gen3/in/cartesian_velocity" kortex_driver::TwistCommand) + (ros::advertise "/arm_gen3/in/joint_velocity" kortex_driver::Base_JointSpeeds) + (ros::advertise "/arm_gen3/in/clear_faults" std_msgs::Empty) + (ros::advertise "/arm_gen3/in/stop" std_msgs::Empty) + (ros::advertise "/arm_gen3/in/emergency_stop" std_msgs::Empty) + self))) + ;; default robot-interface methods + (:default-controller () (send self :arm-controller)) + (:arm-controller () + (list + (append + (cond ((or (derivedp robot gen3_robotiq_2f_140-robot) (derivedp robot gen3_robotiq_2f_85-robot)) + (list + (cons :controller-action + "arm_gen3/gen3_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "arm_gen3/gen3_joint_trajectory_controller/state"))) ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (list + (cons :controller-action + "arm_gen3/gen3_lite_joint_trajectory_controller/follow_joint_trajectory") + (cons :controller-state "arm_gen3/gen3_lite_joint_trajectory_controller/state")))) ;; @TODO: This is dummy topoic name. The kinova_driver did not publish JointTrajectoryControllerState + (list (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :joint-list) :name)))))) + (:go-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :go-grasp-robotiq-2f-140 args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :go-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :go-grasp-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + (:start-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :start-grasp-robotiq-2f-140 args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :start-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :start-grasp-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + (:stop-grasp + (&rest args) + (cond ((derivedp robot gen3_robotiq_2f_140-robot) + (send* self :stop-grasp-robotiq-2f-140 args)) + ((derivedp robot gen3_robotiq_2f_85-robot) + (send* self :stop-grasp-robotiq-2f-85 args)) + ((derivedp robot gen3_lite_gen3_lite_2f-robot) + (send* self :stop-grasp-lite args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" robot)))) + ;; grasp for robotiq 2f 85 + (:go-grasp-robotiq-2f-85 (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro + (let ((pos-deg (rad2deg pos))) + (send robot :finger_joint :joint-angle pos-deg) + (send robot :left_inner_finger_joint :joint-angle (* -1 pos-deg)) + (send robot :left_inner_knuckle_joint :joint-angle pos-deg) + (send robot :right_inner_finger_joint :joint-angle (* -1 pos-deg)) + (send robot :right_inner_knuckle_joint :joint-angle pos-deg) + (send robot :right_outer_knuckle_joint :joint-angle pos-deg)) + (return-from :go-grasp-robotiq-2f-85 t)) + (let ((pos-max 0.8) (pos-min 0.0)) + (when (or (< pos pos-min) (> pos pos-max)) + (ros::ros-warn (format nil ":pos ~A is out of range." pos)) + (setq pos (max pos-min (min pos pos-max))))) + (let (goal result) + (setq goal (instance control_msgs::GripperCommandActionGoal :init)) + (send goal :goal :command :position pos) + (send gripper-action :send-goal goal) + (when wait (send gripper-action :wait-for-result)) + (setq result (send gripper-action :get-result)) + result)) + (:start-grasp-robotiq-2f-85 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-85 :pos 0.8 args)) + (:stop-grasp-robotiq-2f-85 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-85 :pos 0.0 args)) + ;; grasp for robotiq 2f 140 + (:go-grasp-robotiq-2f-140 (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro + (let ((pos-deg (rad2deg pos))) + (send self :finger_joint :joint-angle pos-deg) + (send self :left_inner_finger_joint :joint-angle pos-deg) + (send self :left_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_inner_finger_joint :joint-angle pos-deg) + (send self :right_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_outer_knuckle_joint :joint-angle (* -1 pos-deg))) + (return-from :go-grasp-robotiq-2f-140 t)) + (let ((pos-max 0.69) (pos-min 0.0)) + (when (or (< pos pos-min) (> pos pos-max)) + (ros::ros-warn (format nil ":pos ~A is out of range." pos)) + (setq pos (max pos-min (min pos pos-max))))) + (let (goal result) + (setq goal (instance control_msgs::GripperCommandActionGoal :init)) + (send goal :goal :command :position pos) + (send gripper-action :send-goal goal) + (when wait (send gripper-action :wait-for-result)) + (setq result (send gripper-action :get-result)) + result)) + (:start-grasp-robotiq-2f-140 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-140 :pos 0.69 args)) + (:stop-grasp-robotiq-2f-140 + (&rest args &key &allow-other-keys) + (send* self :go-grasp-robotiq-2f-140 :pos 0.0 args)) + ;; grasp for gen3 lite + (:go-grasp-lite (&key (pos 0.0) (wait t)) + (when (send self :simulation-modep) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro + (send robot :right_finger_bottom_joint :joint-angle + (rad2deg pos)) + (send robot :right_finger_tip_joint :joint-angle + (rad2deg (+ (* -0.676 pos) 0.149))) + (send robot :left_finger_bottom_joint :joint-angle + (rad2deg (* -1 pos))) + (send robot :left_finger_tip_joint :joint-angle + (rad2deg (+ (* -0.676 pos) 0.149))) + (return-from :go-grasp-lite t)) + (let ((pos-max 0.95) (pos-min -0.05)) + (when (or (< pos pos-min) (> pos pos-max)) + (ros::ros-warn (format nil ":pos ~A is out of range." pos)) + (setq pos (max pos-min (min pos pos-max))))) + (let (goal result) + (setq goal (instance control_msgs::GripperCommandActionGoal :init)) + (send goal :goal :command :position pos) + (send gripper-action :send-goal goal) + (when wait (send gripper-action :wait-for-result)) + (setq result (send gripper-action :get-result)) + result)) + (:start-grasp-lite + (&rest args &key &allow-other-keys) + (send* self :go-grasp-lite :pos -0.05 args)) + (:stop-grasp-lite + (&rest args &key &allow-other-keys) + (send* self :go-grasp-lite :pos 0.95 args)) + ;; :angle-vector-xxx and :check-continuous-joint-move-over-180 + ;; are mainly copied from package://fetcheus/fetch-interface.l + (:check-continuous-joint-move-over-180 ;; can be removed if http//github.com/jsk-ros-pkg/jsk_pr2eus/pull/322 merged + (diff-av) + (let ((i 0) add-new-trajectory-point) + (dolist (j (send robot :joint-list)) + ;; for continuous rotational joint + (when (and (> (- (send j :max-angle) (send j :min-angle)) 360) + (> (abs (elt diff-av i)) 180)) + (ros::ros-warn "continuous joint (~A) moves ~A degree, commanded joint differs from original trajectory to avoid unintentional 360 rotation" (send j :name) (elt diff-av i)) + (setq add-new-trajectory-point t)) + (incf i (send j :joint-dof))) + add-new-trajectory-point)) + (:angle-vector-raw (av &optional (tm 3000) (ctype controller-type) (start-time 0) &rest args) + (let* ((prev-av (send self :state :potentio-vector :wait-until-update t)) + (diff-av (v- av prev-av))) + (when (send self :check-continuous-joint-move-over-180 diff-av) + (let* (avs (minjerk (instance minjerk-interpolator :init)) + (scale-av (send self :sub-angle-vector av prev-av)) + dist div) + (setq dist (abs (geo::find-extream (coerce diff-av cons) #'abs #'>=))) + (setq div (round (/ dist 120.0))) + (send minjerk :reset + :position-list (list prev-av (v+ prev-av scale-av)) + :time-list (list tm)) + (send minjerk :start-interpolation) + (send minjerk :pass-time (/ tm div)) + (dotimes (i div) + (setq avs (append avs (list (send minjerk :pass-time (/ tm div)))))) + (send* self :angle-vector-sequence-raw avs (make-list div :initial-element (/ tm div)) ctype start-time args) + (return-from :angle-vector-raw (car (last avs))))) + (send-super* :angle-vector av tm ctype start-time :minjerk-interpolation t args))) + (:angle-vector-sequence-raw + (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &rest args) + (send-super* :angle-vector-sequence avs tms ctype start-time :minjerk-interpolation t args)) + (:angle-vector + (av &optional (tm 3000) &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method retuns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [rad] +- tm : time to goal in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-time args) (setq start-time (cadr (member :start-time args)))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (and (not (boundp '*co*)) (send self :simulation-modep)) + (return-from :angle-vector (send* self :angle-vector-raw av tm ctype start-time args))) + ;; + (when (not (numberp tm)) + (ros::warn ":angle-vector tm is not a number, use :angle-vector av tm args")) + (send* self :angle-vector-motion-plan av :ctype ctype :move-arm :rarm :total-time tm + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities + :minjerk-interpolation t args))) + (:angle-vector-sequence + (avs &optional tms &rest args) ;; (ctype controller-type) (start-time 0) &rest args + ;; &key (clear-velocities t) &allow-other-keys) + "Send joind angle to robot with self-collision motion planning, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs : sequence of joint angle vector [rad] +- tms : list of time to goal from previous angle-vector point in [msec] +" + (let ((ctype controller-type) (start-time 0) (start-offset-time nil) (clear-velocities t)) + ;; as of 0.3.x, :angle-vector (robot-interface) :acceps tm ctype start-time as optional arguments, but in here we prefer old API + (if (= (length args) 1) ;; args must be ctype + (setq ctype (car args) + args (cdr args))) + (if (and (>= (length args) 2) (null (member (car args) '(:start-time :clear-velocities))));; args must be ctype start-time + (setq ctype (car args) + start-time (cadr args) + args (cddr args))) + (if (member :start-offset-time args) (setq start-offset-time (cadr (member :start-offset-time args)))) + (if (member :clear-velocities args) (setq clear-velocities (cadr (member :clear-velocities args)))) + ;; for simulation mode + (when (send self :simulation-modep) + (return-from :angle-vector-sequence + (send* self :angle-vector-sequence-raw avs tms ctype start-time args))) + (unless (and (listp tms) (every #'numberp tms)) + (ros::warn ":angle-vector-sequence tms is not a list of number, use :angle-vector-sequence av tms args")) + (if tms + (setq tms (apply #'+ tms)) + (setq tms 3000)) + (send* self :angle-vector-motion-plan avs :ctype ctype :move-arm :rarm :total-time tms + :start-offset-time (if start-offset-time start-offset-time start-time) + :clear-velocities clear-velocities + :minjerk-interpolation t args))) + (:joint-velocity (joint-vel) + "Send cartesian joint velocity command. +joint-vel : joint velocity [rad/s]. length must equal to the number of joint. +" + (let ((joint-length (length (send robot :angle-vector))) + (bj (instance kortex_driver::Base_JointSpeeds :init)) + js) + (when (not (eq (length joint-vel) joint-length)) + (ros::ros-error (format nil "joint-vel must be the length of ~A" joint-length)) + (return-from :joint-velocity)) + (dotimes (i joint-length) + (push (instance kortex_driver::JointSpeed :init + :joint_identifier i + :value (elt joint-vel i)) + js)) + (send bj :joint_speeds (reverse js)) + (ros::publish "/arm_gen3/in/joint_velocity" bj))) + (:stop-joint-velocity () + (send self :joint-velocity + (make-list (length (send robot :angle-vector)) :initial-element 0))) + (:cartesian-velocity (linear-vel angular-vel) + "Send cartesian velocity command to end effector +- linear : linear velocity [m/s]. 3-dim float-vector. +- angular : angular velocity [rad/s]. 3-dim float-vector. +" + (let ((tc (instance kortex_driver::TwistCommand :init))) + (send tc :twist :linear_x (elt linear-vel 0)) + (send tc :twist :linear_y (elt linear-vel 1)) + (send tc :twist :linear_z (elt linear-vel 2)) + (send tc :twist :angular_x (elt angular-vel 0)) + (send tc :twist :angular_y (elt angular-vel 1)) + (send tc :twist :angular_z (elt angular-vel 2)) + (ros::publish "/arm_gen3/in/cartesian_velocity" tc))) + (:stop-cartesian-velocity () + (send self :cartesian-velocity #f(0 0 0) #f(0 0 0))) + (:clear-faults () + (ros::publish "/arm_gen3/in/clear_faults" (instance std_msgs::Empty :init))) + (:stop () + (ros::publish "/arm_gen3/in/stop" (instance std_msgs::Empty :init))) + (:emergency-stop () + (ros::publish "/arm_gen3/in/emergency_stop" (instance std_msgs::Empty :init))) + ) + +(defclass kinova-moveit-environment + :super moveit-environment) +(defmethod kinova-moveit-environment + (:init (&key ((:robot rb) *gen3*) &rest args) + (send-super* :init + :robot rb + :frame-id "base_link" + :scene-service "arm_gen3/get_planning_scene" + :planning-service "arm_gen3/plan_kinematic_path" + :execute-service "arm_gen3/execute_kinematic_path" + :query-planner-interface-service "arm_gen3/query_planner_interface" + :planning-scene-world "arm_gen3/planning_scene_world" + :state-validity-service "arm_gen3/check_state_validity" + args)) + (:default-configuration () + (list (list :rarm + (cons :group-name "arm") + (cons :target-link + (send self :search-link-from-name "end_effector_link")) + (cons :joint-list (send robot :arm :joint-list))))) + ) + + +(defun kinova-init (&key (type nil) &rest args &alow-other-keys) + (unless (boundp '*kinova*) (apply #'kinova (if type (list :type type) nil)) (send *kinova* :reset-pose)) + (unless (ros::ok) (ros::roseus "kinova_eus_interface")) + (unless (boundp '*co*) + (ros::ros-warn ";; collision-object-publisher wait for \"arm_gen3/apply_planning_scene\" service for ~A sec~%" 5) + (if (ros::wait-for-service "arm_gen3/apply_planning_scene" 5) + (setq *co* (instance collision-object-publisher + :init + :service-name "arm_gen3/apply_planning_scene" + :scene-service "arm_gen3/get_planning_scene")) + (ros::ros-warn ";; could not find \"arm_gen3/apply_planning_scene\" skip creating *co*~%"))) + (unless (boundp '*ri*) (setq *ri* (instance kinova-interface :init :type type))) + + (ros::spin-once) + (send *ri* :spin-once) + + ;;(when create-viewer (objects (list *kinova*))) + ) + +;; Remove defmethod robot-interface when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 +(defmethod robot-interface + (:angle-vector + (av &optional (tm nil) (ctype controller-type) (start-time 0) &key (scale 1) (min-time 1.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil)) + "Send joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- av : joint angle vector [deg] +- tm : (time to goal in [msec]) + if designated tm is faster than fastest speed, use fastest speed + if not specified, it will use 1/scale of the fastest speed . + if :fast is specified use fastest speed calculated from max speed +- ctype : controller method name +- start-time : time to start moving +- scale : if tm is not specified, it will use 1/scale of the fastest speed +- min-time : minimum time for time to goal +- end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-steps : number of divisions when interpolating end-coords +- minjerk-interpolation : set t if you want to move robot with minjerk interpolation +" + (if end-coords-interpolation + (return-from :angle-vector (send self :angle-vector-sequence (list av) (list tm) ctype start-time :scale scale :min-time min-time :end-coords-interpolation t :end-coords-interpolation-steps end-coords-interpolation-steps))) + (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil + (unless (gethash ctype controller-table) + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector)) + ;;Check and decide tm + (let ((fastest-tm (* 1000 (send self :angle-vector-duration + (send self :state :potentio-vector) av scale min-time ctype)))) + (cond + ;;Fastest time Mode + ((equal tm :fast) + (setq tm fastest-tm)) + ;;Normal Number designated Mode + ((numberp tm) + (if (< tm fastest-tm) + (setq tm fastest-tm))) + ;;Safe Mode (Speed will be 5 * fastest-tm) + ((null tm) + (setq tm (* 5 fastest-tm))) + ;;Error Not Good Input + (t + (ros::ros-error ":angle-vector tm is invalid. args: ~A" tm) + (error ":angle-vector tm is invalid. args: ~A" tm))) + ) + ;; for simulation mode + (when (send self :simulation-modep) + (if av (send self :angle-vector-simulation av tm ctype))) + ;; minjerk interpolation, 0.001 [sec] inverval + (let (r trajpoints) + (if minjerk-interpolation + (progn + (setq r (pos-list-interpolation (list (send robot :angle-vector) av) + (list (/ tm 1000.0)) + 0.001)) + ;; TODO: Set vel and acc + ;; https://github.com/jsk-ros-pkg/jsk_robot/pull/1313#issuecomment-771768492 + (setq trajpoints + (mapcar #'(lambda (pos vel tim acc) + ;; (list pos vel tim acc)) ;; ideal + (list pos (make-list (length vel) :initial-element 0) tim (make-list (length acc) :initial-element 0))) ;; temporary + (cadr (memq :data r)) ;; positions + (cadr (memq :velocity r)) ;; velocities + (cadr (memq :time r)) ;; duration + (cadr (memq :acceleration r)))) ;; accelerations + (nreverse trajpoints)) + (setq trajpoints + (list + (list av ;; positions + (instantiate float-vector (length av)) ;; velocities + (/ tm 1000.0) ;; duration + (instantiate float-vector (length av)))))) ;; accelerations + (send robot :angle-vector av) + (let ((cacts (gethash ctype controller-table))) + (mapcar + #'(lambda (action param) + (send self :send-ros-controller + action (cdr (assoc :joint-names param)) ;; action server and joint-names + start-time ;; start time + trajpoints)) + cacts (send self ctype)))) + av) + (:angle-vector-sequence + (avs &optional (tms (list 3000)) (ctype controller-type) (start-time 0.1) &key (scale 1) (min-time 0.0) (end-coords-interpolation nil) (end-coords-interpolation-steps 10) (minjerk-interpolation nil)) + "Send sequence of joint angle to robot, this method returns immediately, so use :wait-interpolation to block until the motion stops. +- avs: sequence of joint angles(float-vector) [deg], (list av0 av1 ... avn) +- tms: sequence of duration(float) from previous angle-vector to next goal [msec], (list tm0 tm1 ... tmn) + if tms is atom, then use (list (make-list (length avs) :initial-element tms))) for tms + if designated each tmn is faster than fastest speed, use fastest speed + if tmn is nil, then it will use 1/scale of the fastest speed . + if :fast is specified, use fastest speed calculated from max speed +- ctype : controller method name +- start-time : time to start moving +- scale : if tms is not specified, it will use 1/scale of the fastest speed +- min-time : minimum time for time to goal +- end-coords-interpolation : set t if you want to move robot in cartesian space interpolation +- end-coords-interpolation-steps : number of divisions when interpolating end-coords +- minjerk-interpolation : set t if you want to move robot with minjerk interpolation + " + (setq ctype (or ctype controller-type)) ;; use default controller-type if ctype is nil + (unless (gethash ctype controller-table) + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector-sequence)) + (send self :spin-once) ;; for :state :potentio-vector + (let ((st 0) (traj-points nil) + (av-prev (send self :state :potentio-vector)) av av-next + tm tm-next fastest-tm + (vel (instantiate float-vector (length (car avs))))) + (if (atom tms) (setq tms (make-list (length avs) :initial-element tms))) + (cond + ((< (length tms) (length avs)) + (nconc tms (make-list (- (length avs) (length tms)) :initial-element (car (last tms))))) + ((> (length tms) (length avs)) + (ros::ros-warn "length of tms should be the same or smaller than avs") + (setq tms (subseq tms 0 (length avs))))) + (when end-coords-interpolation ;; set end-coords interpolation + (setq min-time (/ (float min-time) end-coords-interpolation-steps)) + (let ((av-orig (send robot :angle-vector)) ;; initial av, restore at end + (c-orig (send robot :copy-worldcoords)) ;; inital coords, restore at end + (av-prev-orig av-prev) ;; prev-av + (diff-prev (instantiate float-vector (length av-prev))) diff ;; for over 360 deg turns + (limbs '(:larm :rarm :lleg :rleg)) ;; do not move :head and :torso by IK + target-limbs + (minjerk (instance minjerk-interpolator :init)) + end-coords-prev end-coords-current ec-prev ec-current + interpolated-avs interpolated-tms + tm pass-tm i p (ret t)) ;; if nil failed to interpolate + ;; set prev-av + (send robot :angle-vector av-prev) + (setq end-coords-prev (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + ;; choose moved end-coords + (setq i 0) + (dolist (av avs) + (send robot :angle-vector av) + (setq end-coords-current (mapcar #'(lambda (limb) (send robot limb :end-coords :copy-worldcoords)) limbs)) + (setq diff (v- (v- av (setq av (v+ av-prev (send self :sub-angle-vector av av-prev)))) diff-prev)) + (setq target-limbs nil) + (setq tm (elt tms i)) + (if (equal tm :fast) + (setq tm (* 1000 (send self :angle-vector-duration + av-prev av scale min-time ctype)))) + (setq pass-tm (/ tm (float end-coords-interpolation-steps))) + (dotimes (l (length limbs)) + (setq ec-prev (elt end-coords-prev l) ec-current (elt end-coords-current l)) + (when (and ec-prev ec-current + (or (> (norm (send ec-prev :difference-position ec-current)) 1) + (> (norm (send ec-prev :difference-rotation ec-current)) (deg2rad 1)))) + (push (elt limbs l) target-limbs))) + (send minjerk :reset :position-list (list #f(0) #f(1)) :time-list (list tm)) + (send robot :angle-vector av-prev) + (if target-limbs + (progn + (send minjerk :start-interpolation) + (send minjerk :pass-time pass-tm) + (while (progn (send minjerk :pass-time pass-tm) (send minjerk :interpolatingp)) + (setq p (elt (send minjerk :position) 0)) + ;; set midpoint of av as initial pose of IK + (send robot :angle-vector (midpoint p av-prev av)) + (dolist (limb target-limbs) + (setq ec-prev (elt end-coords-prev (position limb limbs)) + ec-current (elt end-coords-current (position limb limbs))) + (setq ret (and ret + (send robot limb :inverse-kinematics (midcoords p ec-prev ec-current))))) + (push (v++ diff-prev (scale p diff) (send robot :angle-vector)) interpolated-avs) + (push pass-tm interpolated-tms) + ) + (push (v++ diff-prev diff av) interpolated-avs) + (push pass-tm interpolated-tms) + ) + (progn + (push av interpolated-avs) + (push tm interpolated-tms))) + (setq end-coords-prev end-coords-current) + (setq av-prev av) + (setq diff-prev diff) + (incf i)) ;; dolist (av avs) + ;; restore states + (setq avs (nreverse interpolated-avs) tms (nreverse interpolated-tms)) + (send robot :move-to c-orig :world) + (send robot :angle-vector av-orig) + (setq av-prev av-prev-orig) + (unless ret + (warning-message 1 ":angle-vector-sequence failed to generate end-coords-interpolation motion~%") + (return-from :angle-vector-sequence nil)) + )) + (prog1 ;; angle-vector-sequence returns avs + avs + ;; minjerk interpolation, 0.001 [sec] inverval + (if minjerk-interpolation + (let (r) + (setq r (pos-list-interpolation (append (list (send robot :angle-vector)) avs) + (mapcar #'(lambda (x) (/ x 1000.0)) tms) + 0.001)) + ;; TODO: Set vel and acc + (setq traj-points + (mapcar #'(lambda (pos vel tim acc) + ;; (list pos vel tim acc)) ;; ideal + (list pos (make-list (length vel) :initial-element 0) tim (make-list (length acc) :initial-element 0))) ;; temporary + (cadr (memq :data r)) ;; positions + (cadr (memq :velocity r)) ;; velocities + (cadr (memq :time r)) ;; duration + (cadr (memq :acceleration r)))) ;; accelerations + (nreverse traj-points)) + (while avs + (setq av (pop avs)) + (setq fastest-tm (* 1000 (send self :angle-vector-duration av-prev av scale min-time ctype))) + (setq tm (pop tms)) + (cond + ((equal tm :fast) + (setq tm fastest-tm)) + ((null tm) + (setq tm (* 5 fastest-tm))) + ((numberp tm) + (if (< tm fastest-tm) + (setq tm fastest-tm))) + (t (ros::ros-error ":angle-vector-sequence tm is invalid. args: ~A" tm) + (error ":angle-vector-sequence tm is invalid. args: ~A" tm))) + (if (car tms) + (progn + (setq tm-next ( car tms)) + (setq fastest-tm-next (* 1000 (send self :angle-vector-duration av (car avs) scale min-time ctype))) + (cond + ((equal tm-next :fast) + (setq tm-next fastest-tm-next)) + ((null tm) + (setq tm (* 5 fastest-tm))) + ((numberp tm-next) + (if (< tm-next fastest-tm-next) + (setq tm-next fastest-tm-next))) + (t (ros::ros-error ":angle-vector-sequence tm is invalid. args: ~A" tm) + (error ":angle-vector-sequence tm is invalid. args: ~A" tm))) + ) + (setq tm-next 1) + ) + (if (and (setq av-next (car avs)) (> tm 0) (> tm-next 0)) + (let ((v0 (send self :sub-angle-vector av av-prev)) + (v1 (send self :sub-angle-vector av-next av))) + (dotimes (i (length vel)) + (setf (elt vel i) + (if (>= (* (elt v0 i) (elt v1 i)) 0) + (* 0.5 (+ (* (/ 1000.0 tm) (elt v0 i)) + (* (/ 1000.0 tm-next) (elt v1 i)))) + 0.0))) + ) + (fill vel 0)) + ;; for simulation mode + (if (send self :simulation-modep) + (send self :angle-vector-simulation av tm ctype)) + ;; + ;; update only joints with in current controller instead of (send robot :angle-vector av) + (unless (send self :simulation-modep) + (let* ((joint-names (flatten (mapcar #'(lambda (c) (cdr (assoc :joint-names c))) (send self ctype)))) + (joint-ids (mapcar #'(lambda (joint-name) (position joint-name (send robot :joint-list) :test #'(lambda (n j) (equal n (send j :name))))) joint-names))) + (mapcar #'(lambda (name id) + (if (and (send robot :joint name) id (> (length av) id)) + (send (send robot :joint name) :joint-angle (elt av id)) + (warning-message 3 "[robot-interface.l] (angle-vector-sequence) could not find joint-name '~A' (~A) or joint-id ~A (av length ~A)~%" name (send robot :joint name) id (length av)))) + joint-names joint-ids))) +; (when (send self :simulation-modep) +; (send self :publish-joint-state) +; (if viewer (send self :draw-objects))) + + (push (list av + (copy-seq vel) ;; velocities + (/ (+ st tm) 1000.0) ;; tm + duration + (instantiate float-vector (length av))) ;; accelerations + traj-points) + (setq av-prev av) + (incf st tm))) + ;; + (let ((cacts (gethash ctype controller-table))) + (unless cacts + (warn ";; controller-type: ~A not found" ctype) + (return-from :angle-vector-sequence)) + (mapcar + #'(lambda (action param) + (send self :send-ros-controller + action (cdr (assoc :joint-names param)) ;; action server and joint-names + start-time ;; start time + traj-points)) + cacts (send self ctype))) + ))) + (:send-ros-controller + (action joint-names starttime trajpoints) + (when (send self :simulation-modep) + (return-from :send-ros-controller nil)) + (if (and warningp + (not (yes-or-no-p (format nil "~C[3~CmAre you sure to move the real robot? (~A) ~C[0m" #x1b 49 (send action :name) #x1b)))) + (return-from :send-ros-controller nil)) + (dolist (name joint-names) + (unless (send robot :joint name) + (warning-message 1 "[robot-interface.l] (send-ros-controller) could not find joint-name '~A' (~A)~%" name (send robot :joint name)) + (return-from :send-ros-controller nil))) + (let* ((goal (send action :make-goal-instance)) + (goal-points nil) + (st (if (numberp starttime) + (ros::time+ (ros::time-now) (ros::time starttime)) + starttime)) + (joints (mapcar #'(lambda (x) + (send robot (intern (string-upcase x) *keyword-package*))) + joint-names))) + (send goal :header :seq 1) + (send goal :header :stamp st) + + (cond + ((equal (class goal) control_msgs::SingleJointPositionActionGoal) + (let* ((joint (car joints)) + (id (position joint (send robot :joint-list))) + (pos (deg2rad (elt (elt (car trajpoints) 0) id))) + ) + (send goal :goal :position pos) + (send goal :goal :max_velocity 5) + (send self :spin-once) + ) + ) + (t + (send goal :goal :trajectory :joint_names joint-names) + (send goal :goal :trajectory :header :stamp st) + (dolist (trajpt trajpoints) + (let* ((all-positions (elt trajpt 0)) + (all-velocities (elt trajpt 1)) + (duration (elt trajpt 2)) + (all-accelerations (elt trajpt 3)) + (positions (instantiate float-vector (length joint-names))) + (velocities (instantiate float-vector (length joint-names))) + (accelerations (instantiate float-vector (length joint-names)))) + (dotimes (i (length joints)) + (let* ((joint (elt joints i)) + (id (position joint (send robot :joint-list))) + p v a) + (setq p (elt all-positions id) + v (elt all-velocities id) + a (elt all-accelerations id)) + (cond + ((derivedp joint rotational-joint) + (setq p (deg2rad p)) + (setq v (deg2rad v)) + (setq a (deg2rad a))) + (t + (setq p (* 0.001 p)) + (setq v (* 0.001 v)) + (setq a (* 0.001 a)))) + (setf (elt positions i) p) + (setf (elt velocities i) v) + (setf (elt accelerations i) a))) + (push (instance trajectory_msgs::JointTrajectoryPoint + :init + :positions positions + :velocities velocities + :accelerations accelerations + :time_from_start (ros::time duration)) + goal-points) + )) + (send self :spin-once) + (send goal :goal :trajectory :points goal-points) + )) + (send action :send-goal goal) + )) + (:send-trajectory (joint-trajectory-msg + &key ((:controller-type ct) controller-type) (starttime 1) (minjerk-interpolation nil) &allow-other-keys) + (unless (gethash ct controller-table) + (warn ";; controller-type: ~A not found" ct) + (return-from :send-trajectory)) + (mapcar + #'(lambda (action param) + (send self :send-trajectory-each + action (cdr (assoc :joint-names param)) ;; action server and joint-names + joint-trajectory-msg + starttime + minjerk-interpolation)) + (gethash ct controller-table) (send self ct))) + (:send-trajectory-each + (action joint-names traj &optional (starttime 0.2) (minjerk-interpolation nil)) + (let* ((jnames (send traj :joint_names)) + (ilst (mapcar #'(lambda (jn) (position jn jnames :test #'string=)) joint-names)) + points-lst) + (when (some #'identity ilst) + (setq ilst (mapcar #'(lambda (jn) + (let ((p (position jn jnames :test #'string=))) + (unless p + (setq p (send robot (intern (string-upcase jn) *keyword-package*)))) + p)) + joint-names)) + (if minjerk-interpolation + ;; minjerk interpolation, 0.001 [sec] inverval + (let ((points (send traj :points)) (tm-prev 0.0) avs tms r) + (setq avs (mapcar #'(lambda (x) (send x :positions)) points)) + (dolist (tm (mapcar #'(lambda (x) (send (send x :time_from_start) :to-sec)) points)) + (setq tms (append tms (list (- tm tm-prev)))) + (setq tm-prev tm)) + (setq r (pos-list-interpolation avs (cdr tms) 0.001)) + (setq points-lst + ;; TODO: Set vel and acc + (mapcar #'(lambda (pos vel acc tim) + (instance trajectory_msgs::JointTrajectoryPoint :init + :positions pos + ;; :velocities vel ;; ideal + :velocities (make-list (length vel) :initial-element 0) ;; temporary + ;; :accelerations acc ;; ideal + :accelerations (make-list (length acc) :initial-element 0) ;; temporary + :time_from_start (ros::time tim))) + (cadr (memq :data r)) + (cadr (memq :velocity r)) + (cadr (memq :acceleration r)) + (cadr (memq :time r)))) + (nreverse points-lst) + ) + (dolist (p (send traj :points)) + (let ((poss (send p :positions)) + (vels (send p :velocities)) + (effs (send p :accelerations)) + plst vlst elst) + (dolist (i ilst) + (cond + ((numberp i) + (push (elt poss i) plst) + (if (and vels (> (length vels) i)) (push (elt vels i) vlst)) + (if (and effs (> (length effs) i)) (push (elt effs i) elst))) + (t + (push (send i :ros-joint-angle) plst) + (if vels (push 0 vlst)) + (if effs (push 0 elst)) + (ros::ros-warn ";; trajectory contains lacking joint names") + ))) + (push + (instance trajectory_msgs::JointTrajectoryPoint :init + :positions (coerce (nreverse plst) float-vector) + :velocities (if vels (coerce (nreverse vlst) float-vector)) + :accelerations (if effs (coerce (nreverse elst) float-vector)) + :time_from_start (send p :time_from_start)) points-lst) + )) + ) + (let ((goal (send action :make-goal-instance)) + (st (ros::time+ (ros::time-now) (ros::time starttime)))) + (send goal :header :stamp (ros::time-now)) + (send goal :header :seq 1) + (send goal :goal :trajectory :header :stamp st) + (send goal :goal :trajectory :header :seq 1) + (send goal :goal :trajectory :joint_names joint-names) + (send goal :goal :trajectory :points (nreverse points-lst)) + (send self :spin-once) + (send action :send-goal goal)) + (apply-trajectory_point joint-names (car (last points-lst)) robot) + ) ;;; /when ilst + )) + ) + +;; Remove below when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460 +(defun controller-result-error-to-string (i) + (elt '(SUCCESSFUL INVALID_GOAL INVALID_JOINTS OLD_HEADER_TIMESTAMP PATH_TOLERANCE_VIOLATED GOAL_TOLERANCE_VIOLATED) (* -1 i))) + +(defmethod controller-action-client + (:action-result-cb + (msg) + ;; The following error messages will not appear for the first (send *ri* :angle-vector) + ;; To display it, use :wait-interpolation or (ros::spin-once groupname) + (let ((error-code (send (send msg :result) :error_code))) + (when (< error-code 0) + ;; Remove the following two lines when below is merged + ;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/460 + (ros::ros-error "~A~%" (send (send msg :result) :error_string)) + (ros::ros-error "error_code: ~A~%" (cons error-code (controller-result-error-to-string error-code))) + (ros::ros-error "For detail, see https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_kinova_robot/README.md#trouble-shooting"))) + (send-super :action-result-cb msg))) + +;; Remove below when below is merged +;; https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/456 +(defun pos-list-interpolation + (pos-list ;; (list pos_1 pos_2 ... pos_N), pos_i is float-vector + time-list ;; (list dtime_1 dtime_2 ... dtime_{N-1}), dtime_i is time[s] between time at pos_{i+1} - pos_i + dt ;; dt [s] + &key (interpolator-class minjerk-interpolator) + ((:interpolator ip) (instance interpolator-class :init)) + (initial-time 0.0) (neglect-first) (vel-vector-list) (acc-vector-list)) + (let* ((data-list) (tm-list) (vel-data-list) (acc-data-list)) + (assert (= (length pos-list) (1+ (length time-list))) + (format nil "check length of pos-list(~A) and tm-list(~A)" + (length pos-list) (length time-list))) + (setq vel-vector-list + (reverse + (do ((i 0 (1+ i)) (vel-list)) + ((> i (length time-list)) vel-list) + (if (or (= i 0) (= i (length time-list))) + (push (instantiate float-vector (length (car pos-list))) vel-list) + (let* ((v0 (scale (/ 1.0 (elt time-list (1- i))) + (v- (elt pos-list i) (elt pos-list (1- i))))) + (v1 (scale (/ 1.0 (elt time-list i)) + (v- (elt pos-list (1+ i)) (elt pos-list i)))) + (v (scale 0.5 (v+ v0 v1)))) + (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0))) + (push v vel-list)))))) + (setq acc-vector-list + (reverse + (do ((i 0 (1+ i)) (acc-list)) + ((> i (length time-list)) acc-list) + (if (or (= i 0) (= i (length time-list))) + (push (instantiate float-vector (length (car vel-vector-list))) acc-list) + (let* ((v0 (scale (/ 1.0 (elt time-list (1- i))) + (v- (elt vel-vector-list i) (elt vel-vector-list (1- i))))) + (v1 (scale (/ 1.0 (elt time-list i)) + (v- (elt vel-vector-list (1+ i)) (elt vel-vector-list i)))) + (v (scale 0.5 (v+ v0 v1)))) + (dotimes (i (length v)) (if (< (* (elt v0 i) (elt v1 i)) 0) (setf (elt v i) 0))) + (push v acc-list)))))) + ;; (format t "=INPUT~%") + ;; (format t "time ~A~%" time-list) + ;; (format t " pos ~A~%" pos-list) + ;; (format t " vel ~A~%" vel-vector-list) + ;; (format t " acc ~A~%" acc-vector-list) + (send* ip :reset + :position-list pos-list + :time-list (let (r) (dolist (n time-list) (push (+ n (if r (car r) 0)) r)) (nreverse r)) ;; list of time[sec] from start for each control point + (append + (if vel-vector-list (list :velocity-list vel-vector-list)) + (if acc-vector-list (list :acceleration-list acc-vector-list)))) + (send ip :start-interpolation) + (while (send ip :interpolatingp) + (push (if (send ip :interpolatingp) + (+ initial-time (send ip :time)) + (+ dt (car tm-list))) tm-list) + (send ip :pass-time dt) + (push (send ip :position) data-list) + (if (find-method ip :velocity) (push (send ip :velocity) vel-data-list)) + (if (find-method ip :acceleration) (push (send ip :acceleration) acc-data-list)) + ) + ;; (format t "=OUTPUT~%") + (if (and vel-data-list acc-data-list) + (mapcar #'(lambda (tm pos vel acc) + ;; (format t "~7,5f ~7,3f ~13,1f ~13,1f~%" + ;; tm (elt pos 0) (elt vel 0) (elt acc 0)) + ) + (reverse tm-list) (reverse data-list) (reverse vel-data-list) (reverse acc-data-list))) + (append + (list :data (if neglect-first (cdr (reverse data-list)) (reverse data-list)) + :time (if neglect-first (cdr (reverse tm-list)) (reverse tm-list))) + (if (find-method ip :velocity) + (list :velocity (if neglect-first (cdr (reverse vel-data-list)) (reverse vel-data-list)))) + (if (find-method ip :acceleration) + (list :acceleration (if neglect-first (cdr (reverse acc-data-list)) (reverse acc-data-list)))) + ))) + +;; This method can be removed if below is merged +;; https://github.com/euslisp/jskeus/pull/596 +(defmethod interpolator + (:pass-time + (dt) + "process interpolation for dt[sec]" + (when interpolatingp + (setq position (send self :interpolation)) + (incf time dt) + (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list)))) + (when (eps> time (nth segment time-list) (* 0.1 dt)) + ;; if time-segment is not aligned, need to fix the data (see https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/457) + (while (and (< segment segment-num) (eps> time (nth segment time-list) (* 0.1 dt))) + (setq segment-time (- time (nth segment time-list))) + (incf segment))) + (when (>= segment segment-num) + ;; adjust time and segment-time to exact position + (setq segment (1- segment-num)) + (setq time (car (last time-list))) + (setq segment-time (- time (if (= segment 0) 0 (nth (1- segment) time-list)))) + ;; re-calculate :interpolation + (setq position (send self :interpolation)) + (send self :reset)) + position)) + ) + +;; This method can be removed if below is merged +;; https://github.com/euslisp/jskeus/pull/596 +(defmethod minjerk-interpolator + (:interpolation + () + "Minjerk interpolator, a.k.a Hoff & Arbib + Example code is: +(setq l (instance minjerk-interpolator :init)) +(send l :reset :position-list (list #f(1 2 3) #f(3 4 5) #f(1 2 3)) :time-list (list 0.1 0.18)) +(send l :start-interpolation) +(while (send l :interpolatingp) (send l :pass-time 0.02) (print (send l :position))) +" + (let* ((xi (nth segment position-list)) + (xf (nth (1+ segment) position-list)) + (vi (nth segment velocity-list)) + (vf (nth (1+ segment) velocity-list)) + (ai (nth segment acceleration-list)) + (af (nth (1+ segment) acceleration-list)) + ;; + (t1+t2 (- (nth segment time-list) (if (> segment 0) (nth (1- segment) time-list) 0))) ;; total time of segment + ;; A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t) + ;; B=(gv-(v+a*t))/(t*t) + ;; C=(ga-a)/t + (A (scale (/ 1.0 (* t1+t2 t1+t2 t1+t2)) (v- xf (reduce #'v+ (list xi (scale t1+t2 vi) (scale (* t1+t2 t1+t2) (scale 0.5 ai))))))) + (B (scale (/ 1.0 (* t1+t2 t1+t2)) (v- vf (v+ vi (scale t1+t2 ai))))) + (C (scale (/ 1.0 (* t1+t2)) (v- af ai))) + ;; a0=x + ;; a1=v + ;; a2=a/2.0 + ;; a3=10*A-4*B+0.5*C + ;;; a4=(-15*A+7*B-C)/t + ;; a5=(6*A-3*B+0.5*C)/(t*t) + (a0 xi) + (a1 vi) + (a2 (scale 0.5 ai)) + (a3 (v+ (v- (scale 10 A) (scale 4 B)) (scale 0.5 C))) + (a4 (scale (/ 1.0 t1+t2) (v- (v+ (scale -15 A) (scale 7 B)) C))) + (a5 (scale (/ 1.0 t1+t2 t1+t2) (v+ (v+ (scale 6 A) (scale -3 B)) (scale 0.5 C)))) + ) + ;; x=a0+a1*t+a2*t*t+a3*t*t*t+a4*t*t*t*t+a5*t*t*t*t*t + ;; v=a1+2*a2*t+3*a3*t*t+4*a4*t*t*t+5*a5*t*t*t*t + ;; a=2*a2+6*a3*t+12*a4*t*t+20*a5*t*t*t + (setq position + (reduce #'v+ (list a0 + (scale (expt segment-time 1) a1) (scale (expt segment-time 2) a2) + (scale (expt segment-time 3) a3) (scale (expt segment-time 4) a4) + (scale (expt segment-time 5) a5))) + velocity + (reduce #'v+ (list a1 + (scale (* 2 (expt segment-time 1)) a2) (scale (* 3 (expt segment-time 2)) a3) + (scale (* 4 (expt segment-time 3)) a4) (scale (* 5 (expt segment-time 4)) a5))) + acceleration + (reduce #'v+ (list (scale 2 a2) + (scale (* 6 (expt segment-time 1)) a3) (scale (* 12 (expt segment-time 2)) a4) + (scale (* 20 (expt segment-time 3)) a5)))) + position)) + ; + ) diff --git a/jsk_kinova_robot/kinovaeus/kinova-utils.l b/jsk_kinova_robot/kinovaeus/kinova-utils.l new file mode 100644 index 0000000000..23be601618 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova-utils.l @@ -0,0 +1 @@ +(require :kinova "package://kinovaeus/kinova.l") diff --git a/jsk_kinova_robot/kinovaeus/kinova.l b/jsk_kinova_robot/kinovaeus/kinova.l new file mode 100644 index 0000000000..5efa06c698 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/kinova.l @@ -0,0 +1,69 @@ +(require :gen3_robotiq_2f_85 "package://kinovaeus/gen3_robotiq_2f_85.l") +(require :gen3_robotiq_2f_140 "package://kinovaeus/gen3_robotiq_2f_140.l") +(require :gen3_lite_gen3_lite_2f "package://kinovaeus/gen3_lite_gen3_lite_2f.l") + +(defun kinova (&key (type :gen3_robotiq_2f_140) &rest args &allow-other-keys) + (setq *kinova* (case type + (:gen3_robotiq_2f_85 + (instance* gen3_robotiq_2f_85-robot :init args)) + (:gen3_robotiq_2f_140 + (instance* gen3_robotiq_2f_140-robot :init args)) + (:gen3_lite_gen3_lite_2f + (instance* gen3_lite_gen3_lite_2f-robot :init args)) + (t + (warning-message 1 "unknown kinova robot type ~A~%" type) + ))) + *kinova*) + +(defmethod gen3_robotiq_2f_85-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector + (:go-grasp (&key (pos 0)) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_85/urdf/robotiq_arg2f_85_model_macro.xacro + (let ((pos-deg (rad2deg pos))) + (send self :finger_joint :joint-angle pos-deg) + (send self :left_inner_finger_joint :joint-angle (* -1 pos-deg)) + (send self :left_inner_knuckle_joint :joint-angle pos-deg) + (send self :right_inner_finger_joint :joint-angle (* -1 pos-deg)) + (send self :right_inner_knuckle_joint :joint-angle pos-deg) + (send self :right_outer_knuckle_joint :joint-angle pos-deg))) + (:start-grasp () + (send self :go-grasp :pos 0.8)) + (:stop-grasp () + (send self :go-grasp :pos 0.0)) + ) +(defmethod gen3_robotiq_2f_140-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector + (:go-grasp (&key (pos 0)) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/robotiq_2f_140/urdf/robotiq_arg2f_140_model_macro.xacro + (let ((pos-deg (rad2deg pos))) + (send self :finger_joint :joint-angle pos-deg) + (send self :left_inner_finger_joint :joint-angle pos-deg) + (send self :left_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_inner_finger_joint :joint-angle pos-deg) + (send self :right_inner_knuckle_joint :joint-angle (* -1 pos-deg)) + (send self :right_outer_knuckle_joint :joint-angle (* -1 pos-deg)))) + (:start-grasp () + (send self :go-grasp :pos 0.69)) + (:stop-grasp () + (send self :go-grasp :pos 0.0)) + ) +(defmethod gen3_lite_gen3_lite_2f-robot + (:arm (&rest args) (send* self :rarm args)) ;; enable to call send *kinova* :arm :angle-vector + (:go-grasp (&key (pos 0)) + ;; mimic joint + ;; https://github.com/Kinovarobotics/ros_kortex/blob/kinetic-devel/kortex_description/grippers/gen3_lite_2f/urdf/gen3_lite_2f_macro.xacro + (send self :right_finger_bottom_joint :joint-angle + (rad2deg pos)) + (send self :right_finger_tip_joint :joint-angle + (rad2deg (+ (* -0.676 pos) 0.149))) + (send self :left_finger_bottom_joint :joint-angle + (rad2deg (* -1 pos))) + (send self :left_finger_tip_joint :joint-angle + (rad2deg (+ (* -0.676 pos) 0.149)))) + (:start-grasp () + (send self :go-grasp :pos -0.05)) + (:stop-grasp () + (send self :go-grasp :pos 0.95)) + ) diff --git a/jsk_kinova_robot/kinovaeus/package.xml b/jsk_kinova_robot/kinovaeus/package.xml new file mode 100644 index 0000000000..edd55650a2 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/package.xml @@ -0,0 +1,39 @@ + + + kinovaeus + 1.1.0 + The kinovaeus package + + Naoya Yamaguchi + + BSD + Naoya Yamaguchi + + catkin + + collada_urdf + euscollada + rostest + roseus + pr2eus + pr2eus_moveit + kortex_description + kortex_driver + + roseus + pr2eus + pr2eus_moveit + + kortex_gazebo + + + kortex_control + + + gen3_robotiq_2f_85_move_it_config + gen3_robotiq_2f_140_move_it_config + gen3_lite_gen3_lite_2f_move_it_config + + + + diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l new file mode 100755 index 0000000000..47babc9083 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.l @@ -0,0 +1,80 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; This test is mainly copied from +;; https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/test/test-fetcheus.l + +(load "package://kinovaeus/kinova-interface.l") + +(deftest kinova-kinematic-simulator-test + (dolist (robot-type (list :gen3_robotiq_2f_85 :gen3_robotiq_2f_140 :gen3_lite_gen3_lite_2f)) + (let ((ri (instance kinova-interface :init :type robot-type)) + (kinova (kinova :type robot-type)) + diff-av) + ;; + (format t "check reset-pose") + (send kinova :reset-pose) + (send ri :robot :reset-pose) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + ;; do not care 360 rotaiton + (setq diff-av (send ri :sub-angle-vector (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":reset-pose, diff-av ~A" diff-av)) + + (format t "check :ik #f(300 0 200), do not care about 360 degree rotation") + (send kinova :arm :inverse-kinematics (make-coords :pos #f(300 0 200) :rpy (float-vector 0 pi/2 0))) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + ;; do not care 360 rotaiton + (setq diff-av (send ri :sub-angle-vector (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":ik 300 0 200, diff-av ~A" diff-av)) + + ;; do care 360 rotation + (format t "check :init-pose") + (send kinova :init-pose) ;; reset + (send ri :robot :init-pose) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":init-pose, diff-av ~A" diff-av)) + + ;; 150 is 150 + (format t "check :joint-angle 150, this should goes to 150") + (send kinova :rarm :wrist-y :joint-angle 150) + (send ri :angle-vector (send kinova :angle-vector)) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":joint-angle 150, diff-av ~A" diff-av)) + + (format t "check :joint-angle-sequence 150 0 -150, this should goes to -150") + ;; to send to -150, use :angle-vector-sequence + (send ri :angle-vector-sequence (list + (progn + (send kinova :rarm :wrist-y :joint-angle 150) + (send kinova :angle-vector)) + (progn + (send kinova :rarm :wrist-y :joint-angle 0) + (send kinova :angle-vector)) + (progn + (send kinova :rarm :wrist-y :joint-angle -150) + (send kinova :angle-vector)))) + (send ri :wait-interpolation) + (setq diff-av (v- (send ri :state :potentio-vector) (send kinova :angle-vector))) + (format t ":state ~A, :robot ~A, diff ~A, ~A~%" (send ri :robot :rarm :wrist-y :joint-angle) (send kinova :rarm :wrist-y :joint-angle) (norm diff-av) (eps= (norm diff-av) 0.0 10.0)) + (assert (eps= (norm diff-av) 0.0 10.0) (format nil ":angle-vector-sequence, diff-av ~A" diff-av)) + ))) + +(deftest kinova-init-test + (let () + (kinova-init :type :gen3_robotiq_2f_85) + (kinova-init :type :gen3_robotiq_2f_140) + (kinova-init :type :gen3_lite_gen3_lite_2f))) + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test new file mode 100644 index 0000000000..ecd206569d --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-kinematic-simulator.test @@ -0,0 +1,3 @@ + + + diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l new file mode 100755 index 0000000000..6a90adaf58 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.l @@ -0,0 +1,80 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") +(init-unit-test) + +;; This test is mainly copied from +;; https://github.com/jsk-ros-pkg/jsk_robot/blob/master/jsk_fetch_robot/fetcheus/test/test-fetch-moveit.l + +(load "package://kinovaeus/kinova-interface.l") + +(setq *ri* (instance kinova-interface :init :type :gen3_robotiq_2f_85)) +(setq *kinova* (kinova :type :gen3_robotiq_2f_85)) + +(send *kinova* :reset-pose) + +(deftest test-kinova-moveit () + (let (tm-0 tm-1) + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector #f(0 0 0 0 0 0 0)))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector (send *kinova* :reset-pose) 2000))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector #f(0 0 0 0 0 0 0)))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + + (setq tm-0 (ros::time-now)) + (while (null (send *ri* :angle-vector (send *kinova* :reset-pose) 2000))) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (ros::ros-info "time for duration ~A" (send (ros::time- tm-1 tm-0) :to-sec)) + (assert (> (send (ros::time- tm-1 tm-0) :to-sec) 3) "collsion avoidance motion is too fast") + )) + +(deftest test-kinova-moveit-angle-vector-start-time () + (let (tm-0 tm-1 tm-diff) + (send *ri* :angle-vector (send *kinova* :reset-pose)) + (send *ri* :wait-interpolation) + (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) + (send *ri* :angle-vector (send *kinova* :angle-vector) 2000 nil 4) + (setq tm-0 (ros::time-now)) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec)) + (ros::ros-info "time for duration ~A" tm-diff) + (assert (> tm-diff 4.5) (format nil "start-time is ignored. Traj finishes at ~A" tm-diff)) + (assert (< tm-diff 8) (format nil "start-time is considered multiple times. Traj finishes at ~A" tm-diff)) + )) + +(deftest test-kinova-moveit-angle-vector-sequence-start-time () + (let (avs tm-0 tm-1 tm-diff) + (send *ri* :angle-vector (send *kinova* :reset-pose)) + (send *ri* :wait-interpolation) + (push (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) avs) + (push (send *kinova* :rarm :move-end-pos #f(50 0 0) :world) avs) + (setq avs (reverse avs)) + (send *ri* :angle-vector-sequence avs 2000 nil 4) + (setq tm-0 (ros::time-now)) + (send *ri* :wait-interpolation) + (setq tm-1 (ros::time-now)) + (setq tm-diff (send (ros::time- tm-1 tm-0) :to-sec)) + (ros::ros-info "time for duration ~A" tm-diff) + (assert (> tm-diff 4.5) (format nil "start-time is ignored. Traj finishes at ~A" tm-diff)) + (assert (< tm-diff 8) (format nil "start-time is considered multiple times. Traj finishes at ~A" tm-diff)) + )) + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test new file mode 100644 index 0000000000..83026e8907 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova-moveit.test @@ -0,0 +1,14 @@ + + + + + + + + + + + + + diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova.l b/jsk_kinova_robot/kinovaeus/test/test-kinova.l new file mode 100755 index 0000000000..a63db0dafe --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova.l @@ -0,0 +1,29 @@ +#!/usr/bin/env roseus +(require :unittest "lib/llib/unittest.l") +(require "package://kinovaeus/kinova.l") +;; (require "package://kinovaeus/kinova-interface.l") + +(init-unit-test) + +(deftest test-pose + (let (robot) + (setq robot (kinova)) + (objects (list robot)) + ;; rotation-angle NIL is returned when rotation-matrix is a unit-matrix + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + + (setq robot (kinova :type :gen3_robotiq_2f_140)) + (objects (list robot)) + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps= (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 0)) + + (setq robot (kinova :type :gen3_lite_gen3_lite_2f)) + (objects (list robot)) + (warning-message 2 "~A end-corods ~A~%" (send robot :name) (rotation-angle (send robot :arm :end-coords :worldrot))) + (assert (eps< (norm (send (make-coords) :difference-rotation (send robot :arm :end-coords))) 2.0)) + )) + + +(run-all-tests) +(exit) diff --git a/jsk_kinova_robot/kinovaeus/test/test-kinova.test b/jsk_kinova_robot/kinovaeus/test/test-kinova.test new file mode 100644 index 0000000000..c516ac8d84 --- /dev/null +++ b/jsk_kinova_robot/kinovaeus/test/test-kinova.test @@ -0,0 +1,3 @@ + + +