diff --git a/onekey.sh b/onekey.sh index b7edf8c..795a68c 100755 --- a/onekey.sh +++ b/onekey.sh @@ -10,6 +10,7 @@ export PATH #================================================= GAME_ENABLE="no" +ADV_GAME_ENABLE="no" sh_ver="2.0" filepath=$(cd "$(dirname "$0")"; pwd) Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m" @@ -314,7 +315,18 @@ install_spark(){ #catkin_make install } - +#安装sagittarius机械臂的提示信息 +install_sgr_msg(){ + echo -e "${Info}没有找到sagittarius_moveit, sagittarius_arm_ros未安装?" + echo -e "${Info}" + echo -e "${Info}执行下列指令安装sagittarius_arm_ros, 假设Spark的工作空间路径为~/spark_noetic" + echo -e "${Info}" + echo -e "${Info}cd ~/spark_noetic/src/spark_driver/arm" + echo -e "${Info}git clone https://github.com/NXROBO/sagittarius_ws.git" + echo -e "${Info}rm ./sagittarius_ws/src/CMakeLists.txt" + echo -e "${Info}cd ~/spark_noetic && catkin_make" + echo -e "${Info}" +} #远程设置 @@ -452,14 +464,11 @@ voice_nav(){ } -#机械臂与摄像头匹对标定 -cal_camera_arm(){ - echo -e "${Info}" - echo -e "${Info}机械臂与摄像头匹对标定" +#机械臂与摄像头匹对标定uArm +cal_camera_arm_uarm(){ ROSVER=`/usr/bin/rosversion -d` PROJECTPATH=$(cd `dirname $0`; pwd) source ${PROJECTPATH}/devel/setup.bash - echo -e "${Info}" echo -e "${Info}请确定：" echo -e "${Info} A.摄像头已反向向下安装好。机械臂正常上电。" @@ -471,21 +480,88 @@ cal_camera_arm(){ if [ $ROSVER = "kinetic" ]; then echo -e "${Info}It is kinetic." print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" - roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} + roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} elif [ $ROSVER = "indigo" ]; then echo -e "${Info}It is indigo." print_command "roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" - roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} + roslaunch spark_carry_object spark_carry_cal_cv2.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} elif [ $ROSVER = "melodic" ]; then echo -e "${Info}It is melodic." print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" - roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} + roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} elif [ $ROSVER = "noetic" ]; then echo -e "${Info}It is melodic." print_command "roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" - roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} + roslaunch spark_carry_object spark_carry_cal_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} fi - +} + +#机械臂与摄像头匹对标定 sagittarius +cal_camera_arm_sgr(){ + ROSVER=`/usr/bin/rosversion -d` + PROJECTPATH=$(cd `dirname $0`; pwd) + source ${PROJECTPATH}/devel/setup.bash + + if [ -n "$(roscd sagittarius_moveit)" ]; then + install_sgr_msg + else + echo -e "${Info} " + echo -e "${Info} 请确定：" + echo -e "${Info} A.摄像头已反向向下安装好。" + echo -e "${Info} B.机械臂正常上电。" + echo -e "${Info} 退出请输入：Ctrl + c " + echo -e "${Info} " + echo -e "${Info} 请选择标定模式：" + echo -e "${Info} 1.自动模式(简易版)" + echo -e "${Info} 2.自动模式" + echo -e "${Info} 3.手动模式" + echo && stty erase ^? && read -p "请输入数字 [1-3]：" calinum + case "$calinum" in + 1) + print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}" + roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} + ;; + 2) + print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} fast_mode:=false" + roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} fast_mode:=false + ;; + 3)print_command "roslaunch spark_sagittarius_carry cal_cv3.launch camera_type_tel:=${CAMERATYPE}" + roslaunch spark_sagittarius_carry cal_cv3.launch camera_type_tel:=${CAMERATYPE} + ;; + *) + echo -e "${Error} 错误，默认使用自动模式" + print_command "roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE}" + roslaunch spark_sagittarius_carry cal_cv3_auto.launch camera_type_tel:=${CAMERATYPE} + ;; + esac + fi +} + +#机械臂与摄像头匹对标定 +cal_camera_arm(){ + echo -e "${Info} " + echo -e "${Info} 机械臂与摄像头匹对标定" + ROSVER=`/usr/bin/rosversion -d` + PROJECTPATH=$(cd `dirname $0`; pwd) + source ${PROJECTPATH}/devel/setup.bash + + echo -e "${Info} " + echo -e "${Info} 请选择机械臂：" + echo -e "${Info} 1.Sagittarius" + echo -e "${Info} 2.uArm" + echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum + + case "$armnum" in + 1) + cal_camera_arm_sgr + ;; + 2) + cal_camera_arm_uarm + ;; + *) + echo -e "${Error} 错误，请选择正确的机械臂" + ;; + esac } @@ -648,10 +724,9 @@ spark_intel_movidius(){ exit fi } -#让SPARK通过机械臂进行视觉抓取 -spark_carry_obj(){ - echo -e "${Info}" - echo -e "${Info}让SPARK通过机械臂进行视觉抓取" + +#让SPARK通过机械臂进行视觉抓取 uarm +spark_carry_obj_uarm(){ ROSVER=`/usr/bin/rosversion -d` PROJECTPATH=$(cd `dirname $0`; pwd) source ${PROJECTPATH}/devel/setup.bash @@ -697,7 +772,66 @@ spark_carry_obj(){ print_command "roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" roslaunch spark_carry_object spark_carry_object_only_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} fi +} + +#让SPARK通过机械臂进行视觉抓取 uarm +spark_carry_obj_sgr(){ + ROSVER=`/usr/bin/rosversion -d` + PROJECTPATH=$(cd `dirname $0`; pwd) + source ${PROJECTPATH}/devel/setup.bash + echo -e "${Info}请选择移动的方式： + ${Green_font_prefix}1.${Font_color_suffix} 固定位置移动 + ${Green_font_prefix}2.${Font_color_suffix} 手动地图指定位置导航(未完成。。) + ${Green_font_prefix}3.${Font_color_suffix} 退出请输入：Ctrl + c" + echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum + case "$armnum" in + 1) + MOVETYPE="fix" + ;; + 2) + MOVETYPE="slam" + ;; + *) + echo -e "${Error} 错误，默认使用固定位置移动" + MOVETYPE="fix" + ;; + esac + echo -e "${Info}" + echo -e "${Info}请确定：" + echo -e "${Info} A.机械臂正常上电。" + echo -e "${Info} B.准备好可与标定方块同样颜色的物品。" + echo -e "${Info}退出请输入：Ctrl + c " + echo -e "${Info}" + echo && stty erase ^? && read -p "按回车键（Enter）开始：" + print_command "roslaunch spark_sagittarius_carry carry_object_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE}" + roslaunch spark_sagittarius_carry carry_object_cv3.launch camera_type_tel:=${CAMERATYPE} lidar_type_tel:=${LIDARTYPE} +} + +#让SPARK通过机械臂进行视觉抓取 +spark_carry_obj(){ + echo -e "${Info}" + echo -e "${Info}让SPARK通过机械臂进行视觉抓取" + ROSVER=`/usr/bin/rosversion -d` + PROJECTPATH=$(cd `dirname $0`; pwd) + source ${PROJECTPATH}/devel/setup.bash + echo -e "${Info} " + echo -e "${Info} 请选择机械臂：" + echo -e "${Info} 1.Sagittarius" + echo -e "${Info} 2.uArm" + echo && stty erase ^? && read -p "请输入数字 [1-2]：" armnum + + case "$armnum" in + 1) + spark_carry_obj_sgr + ;; + 2) + spark_carry_obj_uarm + ;; + *) + echo -e "${Error} 错误，请选择正确的机械臂" + ;; + esac } #让SPARK使用激光雷达绘制地图(gmapping) @@ -827,6 +961,54 @@ spark_carry_game(){ esac } +#进阶赛示例程序 +spark_sgr_game(){ + + ROSVER=`/usr/bin/rosversion -d` + PROJECTPATH=$(cd `dirname $0`; pwd) + source ${PROJECTPATH}/devel/setup.bash + if [ -n "$(roscd spark_match)" ]; then + echo -e "${Info}没有找到进阶赛的示例程序" + elif [ -n "$(roscd sagittarius_moveit)" ]; then + install_sgr_msg + else + echo -e "${Info}" + echo -e "${Info}进阶赛比赛示例程序" + echo -e "${Info}请选择方式： + ${Green_font_prefix}1.${Font_color_suffix} 建图 + ${Green_font_prefix}2.${Font_color_suffix} 控制Spark记录地点 + ${Green_font_prefix}3.${Font_color_suffix} USB摄像头标定(只需要标定一次) + ${Green_font_prefix}4.${Font_color_suffix} 运行拾取示例程序 + ${Green_font_prefix}5.${Font_color_suffix} 运行进阶赛示例程序 + ${Green_font_prefix}6.${Font_color_suffix} 退出请输入：Ctrl + c" + echo && stty erase ^? && read -p "请输入数字 [1-5]：" armnum + case "$armnum" in + 1) + roslaunch spark_match spark_match_map.launch + ;; + 2) + gnome-terminal -x bash -c "rosrun spark_match semantic_map.sh" + roslaunch spark_match spark_match_nav.launch enable_arm_tel:="no" + ;; + 3) + gnome-terminal -x bash -c "rosrun spark_match usb_cam_cali.sh" + roslaunch spark_match usb_cam.launch + ;; + 4) + gnome-terminal -x bash -c "rosrun spark_match pickup_demo.sh" + roslaunch spark_match spark_match_nav.launch + ;; + 5) + gnome-terminal -x bash -c "rosrun spark_match run_demo.sh" + roslaunch spark_match spark_match_nav.launch + ;; + *) + echo -e "${Error} 错误，请填入正确的数字" + ;; + esac + fi +} + spark_test_mode(){ PROJECTPATH=$(cd `dirname $0`; pwd) @@ -1053,8 +1235,16 @@ check_camera_game(){ ${Green_font_prefix} 13.${Font_color_suffix} hsv值自动查找" fi echo -e " - ${Green_font_prefix} 20.${Font_color_suffix} 竞赛示例程序 - + ${Green_font_prefix} 20.${Font_color_suffix} 竞赛示例程序" + fi + + if [[ "${ADV_GAME_ENABLE}" == "yes" ]]; then + echo -e " + ${Green_font_prefix} 21.${Font_color_suffix} 进阶赛示例程序" + fi + + if [[ "${GAME_ENABLE}" == "yes" ]] || [[ "${ADV_GAME_ENABLE}" == "yes" ]]; then + echo -e " ————————————" fi } @@ -1165,6 +1355,9 @@ case "$num" in 20) spark_carry_game ;; + 21) + spark_sgr_game + ;; 100) tell_us ;; diff --git a/src/spark/spark_description/urdf/spark_340.urdf.xacro b/src/spark/spark_description/urdf/spark_340.urdf.xacro index eb14092..084a047 100755 --- a/src/spark/spark_description/urdf/spark_340.urdf.xacro +++ b/src/spark/spark_description/urdf/spark_340.urdf.xacro @@ -45,7 +45,7 @@ - + diff --git a/src/spark/spark_description/urdf/spark_sagittarius_descriptions.urdf b/src/spark/spark_description/urdf/spark_sagittarius_descriptions.urdf index e69de29..15982b3 100644 --- a/src/spark/spark_description/urdf/spark_sagittarius_descriptions.urdf +++ b/src/spark/spark_description/urdf/spark_sagittarius_descriptions.urdf @@ -0,0 +1,5 @@ + + + + + diff --git a/src/spark_app/spark_sagittarius_carry/CMakeLists.txt b/src/spark_app/spark_sagittarius_carry/CMakeLists.txt new file mode 100644 index 0000000..f17a3e1 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/CMakeLists.txt @@ -0,0 +1,209 @@ +cmake_minimum_required(VERSION 3.0.2) +project(spark_sagittarius_carry) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + roscpp + rospy + std_msgs + actionlib_msgs + message_generation + actionlib +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# # position.msg +# # status.msg +# ) + +## Generate services in the 'srv' folder +add_service_files( + FILES + scene.srv +) + +## Generate actions in the 'action' folder +add_action_files( + FILES + SGRCtrl.action +# Action2.action +) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + actionlib_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES spark_sagittarius_carry +# CATKIN_DEPENDS roscpp rospy +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/spark_sagittarius_carry.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/spark_sagittarius_carry_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_spark_sagittarius_carry.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/spark_app/spark_sagittarius_carry/action/SGRCtrl.action b/src/spark_app/spark_sagittarius_carry/action/SGRCtrl.action new file mode 100644 index 0000000..8f5eeb3 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/action/SGRCtrl.action @@ -0,0 +1,50 @@ +# Define the goal + +# grasp_type 参数的定义项 +# 设置当前夹爪状态，在动作执行之前设置 +uint8 GRASP_NONE = 0 # 自定义动作下使用，夹爪不做处理 +uint8 GRASP_OPEN = 1 # 打开夹爪, 放下动作 +uint8 GRASP_CLOSE = 2 # 闭合夹爪, 抓取动作 + +# action_type 参数的定义项 +# 需要执行动作的类型，可以指定末端姿态或者自动计算 +# 自定义姿态：是让机械臂到某个目标点 +# 预设动作：是执行某个预设的一系列动作 +uint8 ACTION_TYPE_XYZ = 0 # 自定义姿态：末端姿态使用动态计算 +uint8 ACTION_TYPE_XYZ_RPY = 1 # 自定义姿态：末端姿态使用给定姿态 +uint8 ACTION_TYPE_DEFINE_STAY = 2 # 预设动作：待机状态 +uint8 ACTION_TYPE_PICK_XYZ = 3 # 预设动作：XYZ拾取动作 +uint8 ACTION_TYPE_PICK_XYZ_RPY = 4 # 预设动作：XYZ+RPY拾取动作 +uint8 ACTION_TYPE_PUT_XYZ = 5 # 预设动作：XYZ放下动作 +uint8 ACTION_TYPE_PUT_XYZ_RPY = 6 # 预设动作：XYZ+RPY放下动作 + +uint8 grasp_type # 末端初始化状态 +uint8 action_type # 动作类型 + +# 目标点位置 +float64 pos_x +float64 pos_y +float64 pos_z +float64 pos_roll +float64 pos_pitch +float64 pos_yaw + +--- +# Define the result +uint8 SUCCESS = 0 +uint8 ERROR = 1 +uint8 PREEMPT = 2 +uint8 PLAN_NOT_FOUND = 3 +uint8 GRASP_FAILD = 4 # 抓空 + +# 执行结果 +uint8 result + +--- +# Define a feedback message +uint8 PLANNING = 0 +uint8 EXEC_POSITION = 1 +uint8 EXEC_GRASP = 2 + +# 当前步骤 +uint8 step \ No newline at end of file diff --git a/src/spark_app/spark_sagittarius_carry/config/hsv.yaml b/src/spark_app/spark_sagittarius_carry/config/hsv.yaml new file mode 100644 index 0000000..8025bb7 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/config/hsv.yaml @@ -0,0 +1,6 @@ +h_max: 103 +h_min: 91 +s_max: 255 +s_min: 65 +v_max: 255 +v_min: 63 diff --git a/src/spark_app/spark_sagittarius_carry/launch/cal_cv3.launch b/src/spark_app/spark_sagittarius_carry/launch/cal_cv3.launch new file mode 100755 index 0000000..f3de0b2 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/launch/cal_cv3.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_app/spark_sagittarius_carry/launch/cal_cv3_auto.launch b/src/spark_app/spark_sagittarius_carry/launch/cal_cv3_auto.launch new file mode 100755 index 0000000..75f31ff --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/launch/cal_cv3_auto.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_app/spark_sagittarius_carry/launch/carry_object_cv3.launch b/src/spark_app/spark_sagittarius_carry/launch/carry_object_cv3.launch new file mode 100755 index 0000000..5771bd8 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/launch/carry_object_cv3.launch @@ -0,0 +1,40 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_app/spark_sagittarius_carry/nodes/cali_cam_cv3.py b/src/spark_app/spark_sagittarius_carry/nodes/cali_cam_cv3.py new file mode 100755 index 0000000..82d838e --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/nodes/cali_cam_cv3.py @@ -0,0 +1,348 @@ +#!/usr/bin/python3 +# -*- coding: UTF-8 -*- +import roslib +import sys +import rospy +import cv2 +import numpy as np +import math +import os +import yaml +#import pandas as pd +from std_msgs.msg import String +from sensor_msgs.msg import Image +from cv_bridge import CvBridge, CvBridgeError +from sklearn.linear_model import LinearRegression +#from PIL import Image, ImageDraw, ImageFont +xc = 0 +yc = 0 + +count = 9 +index = 0 +# first_findContours = True +# contours = [] + +arm_cmd_sub = None + +xarray = None +yarray = None +xarray_list = None +yarray_list = None +xc_array = None +yc_array = None + +cali_origin_x = 335 +cali_origin_y = 320 +cali_w = 15 +cali_h = 15 +collect_times = 200 +start_flag = 1 +put_cube = 1 +move_position = 0 +collecting = 0 +c_cnt = 0 +HSV_value = [0, 0, 0] +lower_HSV = np.array([0, 0, 0]) +upper_HSV = np.array([0, 0, 0]) + + +def mean_hsv(img, hsv_value): + HSV = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + hsv_value[0] += np.mean(HSV[:, :, 0]) + hsv_value[1] += np.mean(HSV[:, :, 1]) + hsv_value[2] += np.mean(HSV[:, :, 2]) + return hsv_value + + +def hsv_range(hsv_value): + + H_range = 6 + S_range = 120 + V_range = 120 + + lower_H = int(hsv_value[0] - H_range) + upper_H = int(hsv_value[0] + H_range) + + lower_S = int(hsv_value[1] - S_range) + upper_S = int(hsv_value[1] + S_range) + + lower_V = int(hsv_value[2] - V_range) + upper_V = int(hsv_value[2] + V_range) + + if lower_H < 0: + lower_H = 0 + if upper_H > 180: + upper_H = 180 + + if lower_S < 50: + lower_S = 50 + if upper_S > 255: + upper_S = 255 + + if lower_V < 50: + lower_V = 50 + if upper_V > 255: + upper_V = 255 + + lower_HSV = np.array([lower_H, lower_S, lower_V]) + upper_HSV = np.array([upper_H, upper_S, upper_V]) + return lower_HSV, upper_HSV + + +def test(lower_HSV, upper_HSV, image): + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + mask = cv2.inRange(hsv, lower_HSV, upper_HSV) + mask = cv2.erode(mask, None, iterations=2) + mask = cv2.dilate(mask, None, iterations=2) + mask = cv2.GaussianBlur(mask, (3, 3), 0) + cv2.putText(mask, 'Done! Press q to exit!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2, + (255, 255, 255), 2, cv2.LINE_AA) + cv2.imshow("HSV_img", mask) + + +def image_callback(data): + global xc, yc + global move_position + global collecting + global c_cnt + global HSV_value + global lower_HSV, upper_HSV + global first_findContours + global contours + + if start_flag: + return + # change to opencv + try: + cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print(e) + cv_image_draw = cv_image1.copy() + if(put_cube): + cv2.putText(cv_image_draw, 'please place the blue cube directly under', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'the gripper of the robotic arm!', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'then press the \'ENTER\' ', (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'in another terminal!', (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + c_cnt = 0 + cv2.imshow("win_draw", cv_image_draw) + cv2.waitKey(1) + return + elif(move_position): + cv2.putText(cv_image_draw, 'please move the camera to make ', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'the blue cube color in the rectangle ', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'green box! then press the \'ENTER\' ', (30, 130), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, 'in another terminal!', (30, 180), cv2.FONT_HERSHEY_SIMPLEX, 0.8, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.rectangle(cv_image_draw, (cali_origin_x, cali_origin_y), + (cali_origin_x + cali_w, cali_origin_y + cali_h), (0, 255, 0), 3) + c_cnt = 0 + cv2.imshow("win_draw", cv_image_draw) + cv2.waitKey(1) + return + elif(collecting): + c_cnt = c_cnt+1 + cv2.putText(cv_image_draw, 'collecting the hsv!', (30, 30), cv2.FONT_HERSHEY_SIMPLEX, 1.2, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.putText(cv_image_draw, ' please wait for 5s.', (30, 80), cv2.FONT_HERSHEY_SIMPLEX, 1.2, + (0, 255, 0), 2, cv2.LINE_AA) + cv2.rectangle(cv_image_draw, (cali_origin_x, cali_origin_y), + (cali_origin_x + cali_w, cali_origin_y + cali_h), (0, 255, 0), 3) + frame = cv_image1[cali_origin_y + 5:cali_origin_y + + cali_h-5, cali_origin_x + 5:cali_origin_x + cali_w-5] + HSV_value = mean_hsv(frame, HSV_value) + cv2.imshow("win_draw", cv_image_draw) + cv2.waitKey(1) + + if(c_cnt >= collect_times): + for i in range(len(HSV_value)): + HSV_value[i] = HSV_value[i] / collect_times + print(i, len(HSV_value), HSV_value[i]) + lower_HSV, upper_HSV = hsv_range(HSV_value) + #save_hsv(name, lower_HSV, upper_HSV) + print(lower_HSV, upper_HSV) + collecting = 0 + cv2.destroyWindow("win_draw") + rospy.Subscriber("cali_pix_topic", String, command_callback) + arm_cmd_sub.publish(String("next")) + return + # test(lower_HSV, upper_HSV, cv_image1) + + cv_image_cp = cv_image1.copy() + cv_image_hsv = cv2.cvtColor(cv_image_cp, cv2.COLOR_BGR2HSV) + cv_image_gray = cv2.inRange(cv_image_hsv, lower_HSV, upper_HSV) + # smooth and clean noise + cv_image_gray = cv2.erode(cv_image_gray, None, iterations=2) + cv_image_gray = cv2.dilate(cv_image_gray, None, iterations=2) + cv_image_gray = cv2.GaussianBlur(cv_image_gray, (5, 5), 0) + # detect contour + cv2.imshow("win1", cv_image1) + cv2.imshow("win2", cv_image_gray) + cv2.waitKey(1) + contours, hier = cv2.findContours( + cv_image_gray, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + size = [] + size_max = 0 + for i, c in enumerate(contours): + rect = cv2.minAreaRect(c) + box = cv2.boxPoints(rect) + box = np.int0(box) + x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4 + y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4 + w = math.sqrt((box[0][0] - box[1][0])**2 + (box[0][1] - box[1][1])**2) + h = math.sqrt((box[0][0] - box[3][0])**2 + (box[0][1] - box[3][1])**2) + size.append(w * h) + if size[i] > size_max: + size_max = size[i] + index = i + xc = x_mid + yc = y_mid + + # if first_findContours: + # # for i in list(enumerate(contours))[index][1]: + # # contours.append("%d,%d" % (i[0][0], i[0][1])) + # contours = ["%d,%d" % (int(i[0][0]), int(i[0][1])) for i in list(enumerate(contours))[index][1]] + # rospy.loginfo(contours) + # first_findContours = False + # cv2.imshow("win1", cv_image1) + + +def command_callback(data): + global xc_array + global yc_array + global xarray + global yarray + global index + global lower_HSV, upper_HSV + # s = '' + str(xc) + ' ' + str(yc) + '\n' + # file_pix = open('thefile.txt', 'a') + # file_pix.write(s) + # file_pix.close() + # print(xc, yc) + if index < count: + xc_array[index] = xc + yc_array[index] = yc + xarray[index] = xarray_list[index] + yarray[index] = yarray_list[index] + print("%d/%d,pose x,y: %.4f,%.4f. cam x,y: %d,%d" % + (index+1, count, xarray[index], yarray[index], xc, yc)) + # reshape to 2D array for linear regression + xc_array = xc_array.reshape(-1, 1) + yc_array = yc_array.reshape(-1, 1) + xarray = xarray.reshape(-1, 1) + yarray = yarray.reshape(-1, 1) + index = index + 1 + arm_cmd_sub.publish(String("next")) + if index == count: + Reg_x_yc = LinearRegression().fit(yc_array, xarray) + Reg_y_xc = LinearRegression().fit(xc_array, yarray) + k1 = Reg_x_yc.coef_[0][0] + b1 = Reg_x_yc.intercept_[0] + k2 = Reg_y_xc.coef_[0][0] + b2 = Reg_y_xc.intercept_[0] + s = '' + str(k1) + ' ' + str(b1) + ' ' + str(k2) + ' ' + str(b2) + '\n' + + # contours.append('\n') + # s2 = ' '.join(contours) + filename = os.environ['HOME'] + "/thefile.txt" + file_pix = open(filename, 'w') + file_pix.write(s) + # file_pix.write(s2) + file_pix.close() + print("calibration params path: " + filename) + print("Linear Regression for x and yc is : x = %.5fyc + (%.5f)" % (k1, b1)) + print("Linear Regression for y and xc is : y = %.5fxc + (%.5f)" % (k2, b2)) + index = 0 + try: + with open(rospy.get_param("~hsv_config", ""), "w") as f: + hsv_dist = { + 'h_max':upper_HSV[0].item(), + 'h_min':lower_HSV[0].item(), + 's_max':upper_HSV[1].item(), + 's_min':lower_HSV[1].item(), + 'v_max':upper_HSV[2].item(), + 'v_min':lower_HSV[2].item() + } + yaml.dump(hsv_dist, f) + except: + rospy.logwarn("can't open hsv config file") + rospy.logwarn("HSV (upper, lower)") + rospy.logwarn("H (%3d, %3d)" % (upper_HSV[0].item(), lower_HSV[0].item())) + rospy.logwarn("S (%3d, %3d)" % (upper_HSV[1].item(), lower_HSV[1].item())) + rospy.logwarn("V (%3d, %3d)" % (upper_HSV[2].item(), lower_HSV[2].item())) + print("******************************************************") + print(" finish the calibration. Press ctrl-c to exit ") + print(" 标定完成. 然后 Ctrl-C 退出程序 ") + print("******************************************************") + + +def msg_callback(data): + global start_flag + global put_cube + global move_position + global collecting + if(data.data == "start"): + start_flag = 0 + put_cube = 1 + elif(data.data == "start2"): + put_cube = 0 + move_position = 1 + elif(data.data == "start3"): + move_position = 0 + collecting = 1 + + +def main(): + global arm_cmd_sub + global xarray_list + global yarray_list + global xarray + global yarray + global xc_array + global yc_array + global count + rospy.init_node('image_converter', anonymous=True) + r1 = rospy.Rate(1) # 1s + + if rospy.get_param("~fast_mode", True): + # xarray_list = [0.22, 0.35, 0.35, 0.35, 0.22] + # yarray_list = [-0.1, -0.1, 0, 0.1, 0.1] + # count = 5 + xarray_list = [0.22, 0.35, 0.35] + yarray_list = [0, -0.1, 0.1] + count = 3 + else: + xarray_list = [0.28, 0.22, 0.22, 0.28, 0.35, 0.35, 0.35, 0.28, 0.22] + yarray_list = [0, 0, -0.1, -0.1, -0.1, 0, 0.1, 0.1, 0.1] + count = 9 + xarray = np.zeros(count) + yarray = np.zeros(count) + xc_array = np.zeros(count) + yc_array = np.zeros(count) + + print("Calibration node wait to start----") + # while(start_flag == 0): + # r1.sleep() + # if(start_flag == 0): + # return + + sub1 = rospy.Subscriber("/camera/rgb/image_raw", Image, image_callback) + # sub2 = rospy.Subscriber("cali_pix_topic", String, command_callback) + sub3 = rospy.Subscriber("cali_cmd_topic", String, msg_callback) + arm_cmd_sub = rospy.Publisher('cali_arm_cmd_topic', String, queue_size=5) + + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") + cv2.destroyAllWindows() + + +if __name__ == '__main__': + main() diff --git a/src/spark_app/spark_sagittarius_carry/nodes/cali_pos.py b/src/spark_app/spark_sagittarius_carry/nodes/cali_pos.py new file mode 100755 index 0000000..fac69c8 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/nodes/cali_pos.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 +import sys +import rospy +import time +import _thread +from std_msgs.msg import String + +import actionlib +from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal +start_cali = 0 +go_cali_pos = 0 +next_cali_pos = 0 + + +def msg_callback(data): + global start_cali + global go_cali_pos + global next_cali_pos + + if(data.data == "start"): + start_cali = 1 + elif(data.data == "go"): + go_cali_pos = 1 + elif(data.data == "next"): + next_cali_pos = 1 + + +def talker(threadName, delay): + global start_cali + global go_cali_pos + global next_cali_pos + + # pub1 = rospy.Publisher('position_write_topic', position, queue_size=5) + client = actionlib.SimpleActionClient( + rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction) + client.wait_for_server() + pub2 = rospy.Publisher('cali_pix_topic', String, queue_size=5) + sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback) + goal = SGRCtrlGoal() + goal_stay = SGRCtrlGoal() + r1 = rospy.Rate(1) # 1s + r2 = rospy.Rate(0.05) # 20s + r3 = rospy.Rate(0.2) # 5s + r4 = rospy.Rate(0.4) # 2.5s + r1.sleep() + + goal_stay.grasp_type = goal.GRASP_OPEN + goal_stay.action_type = goal.ACTION_TYPE_DEFINE_STAY + client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + + goal.action_type = goal.ACTION_TYPE_XYZ_RPY + goal.pos_z = -0.1 + goal.pos_pitch = 1.57 + point_list = [ + [0.28, 0], # 第一次用于引导用户调整摄像头位置的姿态 + [0.28, 0], + [0.22, 0], + [0.22, -0.1], + [0.28, -0.1], + [0.35, -0.1], + [0.35, 0], + [0.35, 0.1], + [0.28, 0.1], + [0.22, 0.1] + ] + + print("Calibration pose node wait to start----") + while(start_cali == 0): + r1.sleep() + if(start_cali == 0): + return + + print("start to move the sagittarius----") + for i in range(len(point_list)): + if rospy.is_shutdown(): + break + # r1.sleep() + + # 移动到标定位置，引导用户放置方块的地方 + goal.pos_x = point_list[i][0] + goal.pos_y = point_list[i][1] + client.send_goal_and_wait(goal, rospy.Duration.from_sec(30)) + print("Point %d: %.4f, %.4f" % (i + 1, goal.pos_x, goal.pos_y)) + + + # 等待用户放好方块后，机械臂移动到其他地方，不遮挡摄像头 + while(go_cali_pos == 0): + r1.sleep() + if(go_cali_pos == 0): + return + go_cali_pos = 0 + client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + pub2.publish("") + + # 等待标定数据采集后进行下一个动作 + while(next_cali_pos == 0): + r1.sleep() + if(next_cali_pos == 0): + return + next_cali_pos = 0 + + r4.sleep() + + print("end move----") + r3.sleep() + + +if __name__ == '__main__': + rospy.init_node('cali_pos', anonymous=True) + # sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback) + _thread.start_new_thread(talker, ("Thread-1", 2, )) + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") diff --git a/src/spark_app/spark_sagittarius_carry/nodes/cali_pos_auto.py b/src/spark_app/spark_sagittarius_carry/nodes/cali_pos_auto.py new file mode 100755 index 0000000..619c77a --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/nodes/cali_pos_auto.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +import sys +import rospy +import time +import _thread +from std_msgs.msg import String + +import actionlib +from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal +start_cali = 0 +go_cali_pos = 0 +next_cali_pos = 0 + + +def msg_callback(data): + global start_cali + global go_cali_pos + global next_cali_pos + + if(data.data == "start"): + start_cali = 1 + elif(data.data == "go"): + go_cali_pos = 1 + elif(data.data == "next"): + next_cali_pos = 1 + + +def talker(threadName, delay): + global start_cali + global go_cali_pos + global next_cali_pos + + # pub1 = rospy.Publisher('position_write_topic', position, queue_size=5) + client = actionlib.SimpleActionClient( + rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction) + client.wait_for_server() + pub2 = rospy.Publisher('cali_pix_topic', String, queue_size=5) + sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback) + goal = SGRCtrlGoal() + goal_stay = SGRCtrlGoal() + r1 = rospy.Rate(1) # 1s + r2 = rospy.Rate(0.05) # 20s + r3 = rospy.Rate(0.2) # 5s + r4 = rospy.Rate(0.4) # 2.5s + r1.sleep() + + goal_stay.grasp_type = goal_stay.GRASP_OPEN + goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY + client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + + goal.action_type = goal.ACTION_TYPE_XYZ_RPY + goal.pos_x = 0.28 + goal.pos_z = -0.12 + goal.pos_pitch = 1.57 + + if rospy.get_param("~fast_mode", True): + # point_list = [ + # [0.28, 0], + # [0.22, -0.1], + # [0.35, -0.1], + # [0.35, 0], + # [0.35, 0.1], + # [0.22, 0.1] + # ] + point_list = [ + [0.28, 0], + [0.22, 0], + [0.35, -0.1], + [0.35, 0.1] + ] + else: + point_list = [ + [0.28, 0], # 第一次用于引导用户调整摄像头位置的姿态 + [0.28, 0], + [0.22, 0], + [0.22, -0.1], + [0.28, -0.1], + [0.35, -0.1], + [0.35, 0], + [0.35, 0.1], + [0.28, 0.1], + [0.22, 0.1] + ] + + print("Calibration pose node wait to start----") + while(start_cali == 0): + r1.sleep() + if(start_cali == 0): + return + + client.send_goal_and_wait(goal, rospy.Duration.from_sec(30)) + while(go_cali_pos == 0): + r1.sleep() + if(go_cali_pos == 0): + return + go_cali_pos = 0 + client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + pub2.publish("") + while(next_cali_pos == 0): + r1.sleep() + if(next_cali_pos == 0): + return + next_cali_pos = 0 + + print("start to move the sagittarius----") + for i in range(1, len(point_list)): + if rospy.is_shutdown(): + break + # r1.sleep() + + # 移动到标定位置 + goal.action_type = goal.ACTION_TYPE_PICK_XYZ_RPY + goal.pos_x = point_list[i - 1][0] + goal.pos_y = point_list[i - 1][1] + client.send_goal_and_wait(goal, rospy.Duration.from_sec(30)) + + goal.action_type = goal.ACTION_TYPE_PUT_XYZ_RPY + goal.pos_x = point_list[i][0] + goal.pos_y = point_list[i][1] + client.send_goal_and_wait(goal, rospy.Duration.from_sec(30)) + client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + print("Point %d: %.4f, %.4f" % (i + 1, goal.pos_x, goal.pos_y)) + pub2.publish("") + + # 等待标定数据采集后进行下一个动作 + while(next_cali_pos == 0): + r1.sleep() + if(next_cali_pos == 0): + return + next_cali_pos = 0 + + r4.sleep() + + print("end move----") + r3.sleep() + + +if __name__ == '__main__': + rospy.init_node('cali_pos', anonymous=True) + # sub3 = rospy.Subscriber("cali_arm_cmd_topic", String, msg_callback) + _thread.start_new_thread(talker, ("Thread-1", 2, )) + try: + rospy.spin() + except KeyboardInterrupt: + print("Shutting down") diff --git a/src/spark_app/spark_sagittarius_carry/nodes/s_s_carry_object_cv3.py b/src/spark_app/spark_sagittarius_carry/nodes/s_s_carry_object_cv3.py new file mode 100755 index 0000000..abc5480 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/nodes/s_s_carry_object_cv3.py @@ -0,0 +1,745 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- + +import cv2 +import os +import time +import random +import ctypes + +import yaml +import roslib +import rospy +import smach +import smach_ros +import threading +import _thread +import string +import math +import cv2 +import numpy as np +from geometry_msgs.msg import Twist +from std_msgs.msg import String +from spark_sagittarius_carry.srv import * +from spark_sagittarius_carry.msg import * +from cv_bridge import CvBridge, CvBridgeError +from sensor_msgs.msg import Image + +from tf import TransformListener +import tf + +import actionlib +from actionlib import GoalStatus +from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseActionFeedback +from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal + +from smach import State, StateMachine, Concurrence, Container, UserData +from smach_ros import MonitorState, ServiceState, SimpleActionState, IntrospectionServer +from geometry_msgs.msg import Pose, Point, Quaternion + +move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) + + +class GraspObject(State): + ''' + 抓取物体 功能 + ''' + + def __init__(self): + ''' + 初始化 + ''' + #global sub, pub1, pub2 + + State.__init__(self, outcomes=['succeeded', 'aborted', 'pose_notfound']) + self.task = 'grasp object' + self.is_found_object = False + hsv_path = rospy.get_param("~hsv_config") + try: + with open(hsv_path, "r") as f: + content = yaml.load(f.read()) + self.hsv_upper = np.array([content['h_max'], content['s_max'], content['v_max']]) + self.hsv_lower = np.array([content['h_min'], content['s_min'], content['v_min']]) + except: + rospy.logerr("can't not open hsv file: ", hsv_path) + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + self.is_found_object = False + self.onInit() + rate = rospy.Rate(10) + + while not self.is_found_object: + rate.sleep() + print("not found\n") + print("unregisting sub\n") + self.sub.unregister() + print("unregisted sub\n") + if self.grasp() == GoalStatus.SUCCEEDED: + return 'succeeded' + else: + return 'pose_notfound' + + def onInit(self): + global xc, yc, xc_prev, yc_prev, found_count + xc = 0 + yc = 0 + xc_prev = xc + yc_prev = yc + found_count = 0 + #rospy.init_node('image_converter', anonymous=True) + self.sub = rospy.Subscriber( + "/camera/rgb/image_raw", Image, self.image_cb, queue_size=1) + self.client = actionlib.SimpleActionClient( + rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction) + self.client.wait_for_server() + + def grasp(self): + print("start to grasp\n") + global xc, yc, found_count + # stop function + + filename = os.environ['HOME'] + "/thefile.txt" + file_pix = open(filename, 'r') + s = file_pix.read() + file_pix.close() + print(s) + arr = s.split() + a1 = arr[0] + a2 = arr[1] + a3 = arr[2] + a4 = arr[3] + + a = [0]*2 + b = [0]*2 + a[0] = float(a1) + a[1] = float(a2) + b[0] = float(a3) + b[1] = float(a4) + print('k and b value:', a[0], a[1], b[0], b[1]) + + goal_pick = SGRCtrlGoal() + goal_pick.grasp_type = SGRCtrlGoal.GRASP_OPEN + goal_pick.action_type = SGRCtrlGoal.ACTION_TYPE_PICK_XYZ + goal_pick.pos_z = -0.12 + goal_stay = SGRCtrlGoal() + goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY + + + # go forward + goal_pick.pos_x = a[0] * yc + a[1] + goal_pick.pos_y = b[0] * xc + b[1] + ret = self.client.send_goal_and_wait(goal_pick, rospy.Duration.from_sec(30)) + self.client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + return ret + + def image_cb(self, data): + #print("received image\n") + global xc, yc, xc_prev, yc_prev, found_count + # change to opencv + try: + cv_image1 = CvBridge().imgmsg_to_cv2(data, "bgr8") + except CvBridgeError as e: + print('error') + # change rgb to hsv + cv_image2 = cv2.cvtColor(cv_image1, cv2.COLOR_BGR2HSV) + + # extract blue + # LowerBlue = np.array([88, 90, 80]) + # UpperBlue = np.array([108, 255, 255]) + LowerBlue = self.hsv_lower + UpperBlue = self.hsv_upper + mask = cv2.inRange(cv_image2, LowerBlue, UpperBlue) + cv_image3 = cv2.bitwise_and(cv_image2, cv_image2, mask=mask) + + # gray process + cv_image4 = cv_image3[:, :, 0] + + # smooth and clean noise + blurred = cv2.blur(cv_image4, (9, 9)) + (_, thresh) = cv2.threshold(blurred, 90, 255, cv2.THRESH_BINARY) + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (25, 25)) + cv_image5 = cv2.morphologyEx(thresh, cv2.MORPH_CLOSE, kernel) + cv_image5 = cv2.erode(cv_image5, None, iterations=4) + cv_image5 = cv2.dilate(cv_image5, None, iterations=4) + + edges = cv2.Canny(cv_image5,100,200) + cv2.waitKey(1) + + # detect contour + cv2.imshow("win1", cv_image1) + cv2.imshow("win2", cv_image5) + cv2.imshow("edges", edges) + + + contours, hier = cv2.findContours( + cv_image5, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE) + + # if find contours, pick the biggest box + if len(contours) > 0: + size = [] + size_max = 0 + for i, c in enumerate(contours): + rect = cv2.minAreaRect(c) + box = cv2.boxPoints(rect) + box = np.int0(box) + x_mid = (box[0][0] + box[2][0] + box[1][0] + box[3][0]) / 4 + y_mid = (box[0][1] + box[2][1] + box[1][1] + box[3][1]) / 4 + w = math.sqrt((box[0][0] - box[1][0]) ** + 2 + (box[0][1] - box[1][1]) ** 2) + h = math.sqrt((box[0][0] - box[3][0]) ** + 2 + (box[0][1] - box[3][1]) ** 2) + size.append(w * h) + if size[i] > size_max: + size_max = size[i] + index = i + xc = x_mid + yc = y_mid + # if box is not moving for 20 times + # print found_count + if found_count >= 20: + self.is_found_object = True + else: + # if box is not moving + if abs(xc - xc_prev) <= 2 and abs(yc - yc_prev) <= 2: + found_count = found_count + 1 + else: + found_count = 0 + else: + found_count = 0 + xc_prev = xc + yc_prev = yc + + +class ReleaseObject(State): + ''' + 释放物体 功能 + ''' + + def __init__(self): + ''' + 初始化 + ''' + global pub1, pub2 + State.__init__(self, outcomes=['succeeded', 'aborted']) + self.task = 'release object' + #rospy.init_node('image_converter', anonymous=True) + # pub1 = rospy.Publisher('position_write_topic', position, queue_size=10) + # pub2 = rospy.Publisher('pump_topic', status, queue_size=4) + self.client = actionlib.SimpleActionClient( + rospy.get_param("~arm_name", "sgr532") + '/' + 'sgr_ctrl', SGRCtrlAction) + self.client.wait_for_server() + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + global pub1, pub2 + r1 = rospy.Rate(0.15) # 5s + r2 = rospy.Rate(1) # 1s + # pos = position() + goal_put = SGRCtrlGoal() + goal_put.grasp_type = SGRCtrlGoal.GRASP_NONE + goal_put.action_type = SGRCtrlGoal.ACTION_TYPE_PUT_XYZ + goal_put.pos_x = 0.28 + goal_put.pos_y = 0 + goal_put.pos_z = -0.08 + goal_stay = SGRCtrlGoal() + goal_stay.action_type = goal_stay.ACTION_TYPE_DEFINE_STAY + + self.client.send_goal_and_wait(goal_put, rospy.Duration.from_sec(30)) + self.client.send_goal_and_wait(goal_stay, rospy.Duration.from_sec(30)) + # go forward + # pos.x = 200 + # pos.y = 0 + # pos.z = -40 # -80 + # pub1.publish(pos) + # r1.sleep() + + # # stop pump + # pub2.publish(0) + # r2.sleep() + # r1.sleep() + # # go back home + # pos.x = 120 + # pos.y = 0 + # pos.z = 35 + # pub1.publish(pos) + r1.sleep() + return 'succeeded' + + +# 获得厨房位置 +class GetLocation(State): + ''' + 生成导航位置 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=[ + 'succeeded', 'aborted'], output_keys=['waypoint_out']) + self.nav_pose = [rospy.get_param( + "~a_Pose"), rospy.get_param("~b_Pose")] + self.pose_idx = 0 + + def execute(self, userdata): + ''' + 状态执行函数 + :param userdata: + ''' + self.loc_pose = self.get_pose() + if self.loc_pose is None: + return "aborted" + userdata.waypoint_out = self.loc_pose + return 'succeeded' + + def get_pose(self): + ''' + 获得导航点的位置和姿态 + ''' + self.pose_idx = (self.pose_idx + 1) % 2 + # to generate pose + pose = self.split_str(self.nav_pose[self.pose_idx]) + print(pose) + return pose + + def split_str(self, str): + + loc_array = str.split(',') + if len(loc_array) != 7: + return None + pose = Pose(Point(0, 0, 0), Quaternion(0.0, 0.0, 0.0, 1.0)) + pose.position.x = float(loc_array[0]) + pose.position.y = float(loc_array[1]) + pose.position.z = float(loc_array[2]) + pose.orientation.x = float(loc_array[3]) + pose.orientation.y = float(loc_array[4]) + pose.orientation.z = float(loc_array[5]) + pose.orientation.w = float(loc_array[6]) + + return pose + + +class Nav2Waypoint(State): + ''' + 导航：巡航到指定的地点，通过move base + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded', 'aborted'], + input_keys=['waypoint_in']) + + # Subscribe to the move_base action server + # self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) + # Wait up to 60 seconds for the action server to become available + global move_base + move_base.wait_for_server(rospy.Duration(5)) + + rospy.loginfo("Connected to move_base action server") + + self.goal = MoveBaseGoal() + self.goal.target_pose.header.frame_id = "/map" + + def execute(self, userdata): + ''' + 状态执行函数 + :param userdata:用户数据 + ''' + self.goal.target_pose.pose = userdata.waypoint_in + + # return 'succeeded' + + self.goal.target_pose.header.stamp = rospy.Time.now() + if self.goal.target_pose.pose is None: + return 'aborted' + # Send the goal pose to the MoveBaseAction server + global move_base + move_base.send_goal(self.goal) + + if self.preempt_requested(): + self.service_preempt() + return 'preempted' + rospy.loginfo('wait for result') + + # Allow 1 minute to get there + finished_within_time = move_base.wait_for_result(rospy.Duration(60)) + + # If the robot don't get there in time, abort the goal + if not finished_within_time: + move_base.cancel_goal() + rospy.loginfo("Timed out achieving goal") + return 'aborted' + else: + # We made it! + state = move_base.get_state() + if state == GoalStatus.SUCCEEDED: + rospy.loginfo("Goal succeeded!") + return 'succeeded' + + +class TurnBody(State): + ''' + 转动身体 功能 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded', 'aborted']) + self.task = 'turn body' + self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + self.min_z = rospy.get_param("~turnbody_min_z", 0.2) + self.max_z = rospy.get_param("~turnbody_max_z", 0.6) + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + cmd_vel = Twist() + cmd_vel.angular.z = random.uniform(self.min_z, self.max_z) + print(cmd_vel.angular.z) + rate = rospy.Rate(25) + for i in range(25): + self.cmd_vel_pub.publish(cmd_vel) + rate.sleep() + + # self.cmd_vel_pub.publish(cmd) + return 'succeeded' + + +class Go2Goal(State): + ''' + 转动身体 功能 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded', 'aborted']) + self.task = 'go line' + self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + self.min_z = rospy.get_param("~turnbody_min_z", 0.2) + self.max_z = rospy.get_param("~turnbody_max_z", 0.6) + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + cmd_vel = Twist() + cmd_vel.angular.z = 0.25 + print(cmd_vel.angular.z) + rate = rospy.Rate(10) + for i in range(50): + self.cmd_vel_pub.publish(cmd_vel) + rate.sleep() + + cmd_vel = Twist() + #cmd_vel.angular.z = random.uniform(self.min_z, self.max_z) + cmd_vel.linear.x = 0.15 + print(cmd_vel.angular.z) + #rate = rospy.Rate(10) + for i in range(25): + self.cmd_vel_pub.publish(cmd_vel) + rate.sleep() + + # self.cmd_vel_pub.publish(cmd) + return 'succeeded' + + +class TurnBodyQuar(State): + ''' + 转动身体 功能 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded', 'aborted']) + self.task = 'turn body quar' + self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + self.min_z = rospy.get_param("~turnbody_min_z", 0.2) + self.max_z = rospy.get_param("~turnbody_max_z", 0.6) + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + cmd_vel = Twist() + cmd_vel.angular.z = 0.25 + print(cmd_vel.angular.z) + rate = rospy.Rate(10) + for i in range(100): + self.cmd_vel_pub.publish(cmd_vel) + rate.sleep() + + # self.cmd_vel_pub.publish(cmd) + return 'succeeded' + + +class EndAbort(State): + ''' + 提示结果失败 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded']) + # self.play_sound = PlaySound() + self.task = 'end abort' + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + return 'succeeded' + + +class EndSuccess(State): + ''' + 提示结果成功 + ''' + + def __init__(self): + ''' + 初始化 + ''' + State.__init__(self, outcomes=['succeeded']) + # self.play_sound = PlaySound() + self.task = 'end success' + + def execute(self, userdata): + ''' + 执行状态 + :param userdata: + ''' + self.sayword_pub.publish("任务完成") + return 'succeeded' + + +class CarryObject(): + ''' + 场景：搬运物体 + 功能：将目标运行到指定的位置 + ''' + + def __init__(self): + ''' + 初始化 + ''' + rospy.init_node('s_carry_object_node', anonymous=False) + + rospy.on_shutdown(self.shutdown) + self.switch_status = 0 + self.sm_carry_object = None + self.thr = None + self.init_service_and_topic() + # 控制 + self.stopworld = "停止处理" + self.loaction_name = None + + def scene_status_swith_cb(self, req: sceneResponse): + ''' + 场景状态切换回调函数，用于启动场景或者关闭场景 + :param req:请求参数 + ''' + print("-----------req status:%d" % req.END) + return_flag = -1 + if req.type == req.END: + self.switch_status = 0 + global move_base + move_base.cancel_goal() + for i in range(10): + self.cmd_vel_pub.publish(Twist()) + + if not self.thr is None: + try: + self.terminate_thread(self.thr) + except: + pass + + return_flag = 0 + if req.type == req.RUN: + if self.switch_status == 0: + # thread.start_new_thread(self.timer, (1,1)) + self.thr = threading.Thread(target=self.timer, args=(1, 1)) + self.thr.start() + self.switch_status = 1 + return_flag = 1 + + # self.master_s_pub.publish("",0) + print("s_carry_object status:%d" % return_flag) + return sceneResponse(return_flag) + + def timer(self, no, interval): + ''' + 计时器 + :param no: + :param interval:计时器间隔 + ''' + # time.sleep(interval); + rospy.loginfo("req infomaion, status:%s" % (self.switch_status)) + if self.switch_status == 1: + rospy.loginfo("starting to sm") + self.build_state_machine() + self.execute_sm() + + self.switch_status = 0 + rospy.loginfo("stopped to sm") + thread.exit_thread() + + def terminate_thread(self, thread): + """ + Terminates a python thread from another thread. + 中止结束线程 + :param thread: a threading.Thread instance + """ + try: + if not thread.isAlive(): + return + + exc = ctypes.py_object(SystemExit) + res = ctypes.pythonapi.PyThreadState_SetAsyncExc( + ctypes.c_long(thread.ident), exc) + if res == 0: + raise ValueError("nonexistent thread id") + elif res > 1: + # """if it returns a number greater than one, you're in trouble, + # and you should call it again with exc=NULL to revert the effect""" + ctypes.pythonapi.PyThreadState_SetAsyncExc(thread.ident, None) + raise SystemError("PyThreadState_SetAsyncExc failed") + except : + pass + self.cmd_vel_pub.publish(Twist()) + + def init_service_and_topic(self): + ''' + 初始化服务和话题 + ''' + self.switch_status_service = rospy.Service( + '/s_carry_object', scene, self.scene_status_swith_cb) + self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) + + def build_state_machine(self): + ''' + 建立状态机 + ''' + if not self.sm_carry_object is None: + self.sm_carry_object = None + self.sm_carry_object = StateMachine( + outcomes=['succeeded', 'aborted', 'preempted']) + with self.sm_carry_object: + StateMachine.add('GRASP_OBJECT', GraspObject(), transitions={'succeeded': 'NAV_TO_WAYPOINT', + 'pose_notfound': 'GRASP_OBJECT', + 'aborted': 'END_ABORT'}) + StateMachine.add('GET_LOCATION', GetLocation(), transitions={'succeeded': 'NAV_TO_WAYPOINT', + 'aborted': 'END_ABORT'}, + remapping={'waypoint_out': 'nav_waypoint'}) + # StateMachine.add('NAV_TO_WAYPOINT', Nav2Waypoint(), transitions={'succeeded':'RELEASE_OBJECT','aborted':'END_ABORT'}, + + StateMachine.add('NAV_TO_WAYPOINT', Go2Goal(), transitions={'succeeded': 'RELEASE_OBJECT', + 'aborted': 'END_ABORT'}) + # remapping={'waypoint_in':'nav_waypoint'}) + StateMachine.add('RELEASE_OBJECT', ReleaseObject(), transitions={'succeeded': 'TURN_BODY_QUAR', + 'aborted': 'END_ABORT'}) + #StateMachine.add('TURN_BODY', TurnBody(), transitions={'succeeded':'GRASP_OBJECT','aborted':'END_ABORT'}) + StateMachine.add('TURN_BODY_QUAR', TurnBodyQuar(), transitions={ + 'succeeded': 'GRASP_OBJECT', 'aborted': 'END_ABORT'}) + StateMachine.add('END_ABORT', EndAbort(), transitions={ + 'succeeded': 'succeeded'}) + StateMachine.add('END_SUCESS', EndSuccess(), + transitions={'succeeded': 'succeeded'}) + + def stop_cb(self, msg): + ''' + 停止回调函数 + :param msg: + ''' + # rospy.loginfo("停止指令："+msg.data) + print(msg.data) + if msg.data == self.stopworld: + switch_client = rospy.ServiceProxy('s_carry_object', scene) + type = 0 + param = "" + respon = switch_client(type, param) + + def execute_sm(self): + ''' + 运行状态即机 + ''' + intro_server = IntrospectionServer( + 'sm_carry_object', self.sm_carry_object, '/SM_ROOT') + intro_server.start() + # Execute the state machine + sm_outcome = self.sm_carry_object.execute() + rospy.loginfo('State Machine Outcome: ' + str(sm_outcome)) + intro_server.stop() + + def shutdown(self): + ''' + 关闭状态机 + ''' + rospy.loginfo("Stopping carry object") + rospy.sleep(1) + self.cmd_vel_pub.publish(Twist()) + # rospy.spin() + + +if __name__ == '__main__': + try: + rospy.loginfo("Init carry object") + arm_name = rospy.get_param("~arm_name", "sgr532") + filename = os.environ['HOME'] + "/thefile.txt" + file_pix = open(filename, 'r') + # s = file_pix.read() + # file_pix.close() + # print(s) + # arr = s.split() + s = file_pix.readline() + print(s) + arr = s.split() + # s = file_pix.readline() + # print(s) + # arr2 = s.split() + # print("1111111111111111", arr2) + file_pix.close() + a1 = arr[0] + a2 = arr[1] + a3 = arr[2] + a4 = arr[3] + # a4=arr[3].replace('\n','') #readline 读取文件的时候，默认加上“\n" + print(a1) + print(a2) + print(a3) + print(a4) + n1 = float(a1)*2 + a = [0]*2 + b = [0]*2 + a[0] = float(a1) + a[1] = float(a2) + b[0] = float(a3) + b[1] = float(a4) + print('k and b value:', a[0], a[1], b[0], b[1]) + + CarryObject() + rospy.spin() + except rospy.ROSInterruptException: + rospy.loginfo("Finised carry object") diff --git a/src/spark_app/spark_sagittarius_carry/nodes/sgr_ctrl.py b/src/spark_app/spark_sagittarius_carry/nodes/sgr_ctrl.py new file mode 100755 index 0000000..8233d96 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/nodes/sgr_ctrl.py @@ -0,0 +1,318 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +''' +Descripttion: Saggitarius moveit application action +version: 1.00 +Author: shudong.hong@nxrobo.com +Company: NXROBO (深圳创想未来机器人有限公司) +LastEditors: shudong.hong@nxrobo.com +''' + +import math +import numpy as np +import rospy +import sys +from moveit_msgs.msg import MoveGroupActionFeedback +import moveit_commander +import tf.transformations as transformations + +import actionlib +from spark_sagittarius_carry.msg import SGRCtrlAction, SGRCtrlGoal, SGRCtrlResult, SGRCtrlFeedback +from sdk_sagittarius_arm.srv import ServoRtInfo, ServoRtInfoRequest + +ispython3 = True if sys.version_info.major == 3 else False + + +class MoveItSGRTool: + def __init__(self, init_pose=True, end_effector=None): + moveit_commander.roscpp_initialize(sys.argv) + self.robot_name = rospy.get_param("~robot_name", "sgr532") + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + self.cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + self.arm_group = moveit_commander.MoveGroupCommander('sagittarius_arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + self.gripper = moveit_commander.MoveGroupCommander( + 'sagittarius_gripper') + + # 当运动规划失败后，允许重新规划 + self.arm_group.allow_replanning(False) + + self.reference_frame = rospy.get_namespace()[1:] + 'base_link' + # 设置目标位置所使用的参考坐标系 + self.arm_group.set_pose_reference_frame(self.reference_frame) + + # 设置目标位置所使用的参考坐标系 + self.gripper.set_pose_reference_frame(self.reference_frame) + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + self.arm_group.set_goal_position_tolerance(0.001) + self.arm_group.set_goal_orientation_tolerance(0.001) + + self.gripper.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + self.arm_group.set_max_acceleration_scaling_factor(0.5) + self.arm_group.set_max_velocity_scaling_factor(0.5) + + # 设置末端效应点link的名称 + if (end_effector is not None): + self.arm_group.set_end_effector_link(end_effector) + + # 获取终端link的名称 + self.end_effector_link = self.arm_group.get_end_effector_link() + rospy.loginfo("end effector link: %s" % self.end_effector_link) + + # + rospy.Subscriber("move_group/feedback", MoveGroupActionFeedback, + self._move_group_feedback_callback) + self.moveit_group_status = "IDLE" + + # 控制机械臂先回到初始化位置 + if init_pose: + self.arm_group.set_named_target('home') + self.arm_group.go() + rospy.sleep(1) + self.pose_stay() + + def __del__(self): + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + def _move_group_feedback_callback(self, fb): + self.moveit_group_status = fb.feedback.state + + def isPlanSuccess(self, px, py, pz, roll=0, pitch=0, yaw=0): + self.arm_group.set_pose_target([px, py, pz, roll, pitch, yaw]) + plan = self.arm_group.plan() + + if ispython3: + plan = plan[1] + + return len(plan.joint_trajectory.points) != 0 + + def gripper_catch(self): + self.gripper.set_joint_value_target([-0.021, -0.021]) + ret = self.gripper.go() + if ret: + rospy.sleep(1) + return ret + + def gripper_open(self): + self.gripper.set_joint_value_target([0.0, 0.0]) + ret = self.gripper.go() + if ret: + rospy.sleep(1) + return ret + + def to_pose_eular(self, times, px, py, pz, roll=0, pitch=0, yaw=0): + self.arm_group.set_pose_target([px, py, pz, roll, pitch, yaw]) + plan = self.arm_group.plan() + + if ispython3: # python3 返回的是列表，下标1是 RobotTrajectory，也可以用列表中 error_code:MoveItErrorCodes 判断 + plan = plan[1] + + if len(plan.joint_trajectory.points) != 0: + self.arm_group.execute(plan) + rospy.sleep(times) + return True + else: + rospy.logwarn("plan can not found!") + return False + + def ee_target_offset(self, px, py, pz, roll=0, pitch=0, yaw=0, ee_type='grasp'): + M = transformations.compose_matrix( + angles=[roll, pitch, yaw], translate=[px, py, pz]) + if ee_type == 'grasp': + M1 = np.dot(M, transformations.translation_matrix([-0.07, 0, 0])) + else: + M1 = M + scale, shear, angles, translate, perspective = transformations.decompose_matrix( + M1) + return translate[0], translate[1], translate[2], angles[0], angles[1], angles[2] + + def ee_xyz_get_rpy(self, x, y, z): + # Get yaw by Arctangent of x and y + yaw = math.atan2(y, x) + roll = 0 + pitch = 0 + + # 使用三角函数求 pitch 的近似值 + # 模型：机械臂抽象成等腰三角形，机械臂末端到坐标原点的距离为底，机械臂臂长为两腰总和 + # 求底边角 + a = 0.532 / 2 + b = 0.532 / 2 + c = math.sqrt(x*x + y*y + z*z) + if a + b <= c: + # 三角形不成立 + pitch = 0 + else: + # 已知三角边长求指定角度 + pitch = math.acos((a*a - b*b - c*c)/(-2*b*c)) - math.asin(z / c) + + # 限定范围 + if pitch > 1.57: + pitch = 1.57 + elif pitch < 0: + pitch = 0 + return roll, pitch, yaw + + def pose_stay(self): + self.arm_group.set_joint_value_target( + [0, 1.4, -1.48, 0, 1.6, 0]) + self.arm_group.go() + + def stop(self): + self.arm_group.set_named_target('home') + self.arm_group.go() + rospy.sleep(1) + self.arm_group.set_named_target('sleep') + self.arm_group.go() + rospy.sleep(1) + self.gripper.set_joint_value_target([0, 0]) + self.gripper.go() + rospy.sleep(0.5) + + +class CencelException(Exception): + pass + + +class SGRCtrlActionServer: + def __init__(self) -> None: + self.sgr_tool = MoveItSGRTool() + rospy.wait_for_service('get_servo_info', 3) + self.servo_info_srv = rospy.ServiceProxy('get_servo_info', ServoRtInfo) + self._server = actionlib.SimpleActionServer( + 'sgr_ctrl', SGRCtrlAction, self.execute, False) + self._server.start() + + def execute(self, goal:SGRCtrlGoal): + r = rospy.Rate(1) + + resp = SGRCtrlResult() + fb = SGRCtrlFeedback() + + # 初始化夹爪 + fb.step = fb.EXEC_GRASP + self._server.publish_feedback(fb) + if goal.grasp_type is SGRCtrlGoal.GRASP_CLOSE: + self.sgr_tool.gripper_catch() + elif goal.grasp_type is SGRCtrlGoal.GRASP_OPEN: + self.sgr_tool.gripper_open() + + # 开始规划 + fb.step = fb.PLANNING + self._server.publish_feedback(fb) + if goal.action_type is goal.ACTION_TYPE_DEFINE_STAY: + # 指定动作 + fb.step = fb.EXEC_POSITION + self._server.publish_feedback(fb) + self.sgr_tool.pose_stay() + else: + if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_XYZ_RPY, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY, SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY]: + # 指定末端姿态 + roll = goal.pos_roll + pitch = goal.pos_pitch + yaw = goal.pos_yaw + else: + # 动态计算末端姿态 + roll, pitch, yaw = self.sgr_tool.ee_xyz_get_rpy(goal.pos_x, goal.pos_y, goal.pos_z) + x, y, z, roll, pitch, yaw = self.sgr_tool.ee_target_offset( + goal.pos_x, goal.pos_y, goal.pos_z, + roll, pitch, yaw + ) + + # 检查姿态是否能到达 + try: + if not self.sgr_tool.isPlanSuccess(x, y, z, roll, pitch, yaw): + raise + if goal.action_type in [ + SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, + SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY, + SGRCtrlGoal.ACTION_TYPE_PUT_XYZ, + SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY + ]: + if not self.sgr_tool.isPlanSuccess(x, y, z + 0.04, roll, pitch, yaw): + raise + if not self.sgr_tool.isPlanSuccess(x, y, z + 0.12, roll, pitch, yaw): + raise + except: + rospy.logwarn("Plan could not found") + resp.result = resp.PLAN_NOT_FOUND + self._server.set_aborted(resp) + return + + # 执行规划 + fb.step = fb.EXEC_POSITION + self._server.publish_feedback(fb) + try: + # 抓取与放置动作 + if goal.action_type in [ + SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, + SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY, + SGRCtrlGoal.ACTION_TYPE_PUT_XYZ, + SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY + ]: + if self._server.is_preempt_requested(): + raise CencelException("Preempt !!!") + self.sgr_tool.to_pose_eular(1, x, y, z + 0.04, roll, pitch, yaw) + + if self._server.is_preempt_requested(): + raise CencelException("Preempt !!!") + self.sgr_tool.to_pose_eular(0.2, x, y, z, roll, pitch, yaw) + + fb.step = fb.EXEC_GRASP + self._server.publish_feedback(fb) + if self._server.is_preempt_requested(): + raise CencelException("Preempt !!!") + if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY]: + self.sgr_tool.gripper_catch() + elif goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PUT_XYZ, SGRCtrlGoal.ACTION_TYPE_PUT_XYZ_RPY]: + self.sgr_tool.gripper_open() + + fb.step = fb.EXEC_POSITION + self._server.publish_feedback(fb) + if self._server.is_preempt_requested(): + raise CencelException("Preempt !!!") + self.sgr_tool.to_pose_eular(0.2, x, y, z + 0.12, roll, pitch, yaw) + + # if self._server.is_preempt_requested(): + # raise CencelException("Preempt !!!") + # self.sgr_tool.pose_stay() + + # 夹爪判断 + if goal.action_type in [SGRCtrlGoal.ACTION_TYPE_PICK_XYZ, SGRCtrlGoal.ACTION_TYPE_PICK_XYZ_RPY]: + ret = self.servo_info_srv.call(ServoRtInfoRequest(servo_id=7)) + if abs(ret.payload) < 24: + resp.result = resp.GRASP_FAILD + self._server.set_aborted(resp) + return + + # 执行某个动作 + else: + if self._server.is_preempt_requested(): + raise CencelException("Preempt !!!") + self.sgr_tool.to_pose_eular(0.2, x, y, z, roll, pitch, yaw) + except CencelException as e: + rospy.logwarn(e) + resp.result = resp.PREEMPT + self._server.set_aborted() + except Exception as e: + rospy.logerr(e) + resp.result = resp.ERROR + self._server.set_aborted() + + resp.result = resp.SUCCESS + self._server.set_succeeded(resp) + + +if __name__ == '__main__': + rospy.init_node("sgr_ctrl_action", anonymous=True) + rospy.sleep(5) + + ser = SGRCtrlActionServer() + + rospy.spin() diff --git a/src/spark_app/spark_sagittarius_carry/package.xml b/src/spark_app/spark_sagittarius_carry/package.xml new file mode 100644 index 0000000..cfd752a --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/package.xml @@ -0,0 +1,78 @@ + + + spark_sagittarius_carry + 0.0.0 + The spark_sagittarius_carry package + + + + + shudong.hong + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + actionlib + std_msgs + actionlib_msgs + message_generation + roscpp + rospy + actionlib + std_msgs + actionlib_msgs + message_generation + roscpp + rospy + actionlib + std_msgs + actionlib_msgs + message_runtime + sdk_sagittarius_arm + + + + + + + + diff --git a/src/spark_app/spark_sagittarius_carry/rviz/rviz_d435.rviz b/src/spark_app/spark_sagittarius_carry/rviz/rviz_d435.rviz new file mode 100644 index 0000000..915b294 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/rviz/rviz_d435.rviz @@ -0,0 +1,365 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 219 + - 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: Camera +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 + - 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 + IMU_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_right_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_rear_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + room_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spark_stack: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/usb_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: sgr532/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/Camera + Enabled: true + Image Rendering: background and overlay + Image Topic: /camera/color/image_raw + Name: Camera + Overlay Alpha: 0.5 + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Visibility: + Grid: true + RobotModel: true + Value: false + Zoom Factor: 1 + 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: 2.036864995956421 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.08599551767110825 + Y: -0.17910736799240112 + Z: 0.06584001332521439 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Camera: + collapsed: false + Displays: + collapsed: false + Height: 696 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000021afc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000166000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000001a9000000ae0000001600ffffff000000010000010f000001cbfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001cb000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004f30000003efc0100000002fb0000000800540069006d00650100000000000004f3000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003970000021a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1267 + X: 256 + Y: 96 diff --git a/src/spark_app/spark_sagittarius_carry/scripts/action_carry.sh b/src/spark_app/spark_sagittarius_carry/scripts/action_carry.sh new file mode 100755 index 0000000..29ddb2d --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/action_carry.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +Info="${Green_font_prefix}[信息]${Font_color_suffix}" +echo -e "${Info} 是否现在开始视觉抓取？" +echo -e "${Info} 开始请按按任意键，否则退出请输入：Ctrl + c " +echo && stty erase '^H' && read -p "按任意键开始：" +gnome-terminal -x bash -c "rosservice call /s_carry_object 'type: 1'" diff --git a/src/spark_app/spark_sagittarius_carry/scripts/cmd_spark_sgr_carry_start.sh b/src/spark_app/spark_sagittarius_carry/scripts/cmd_spark_sgr_carry_start.sh new file mode 100755 index 0000000..5b4f6c4 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/cmd_spark_sgr_carry_start.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +BASEPATH=$(cd `dirname $0`; pwd) +gnome-terminal -x bash -c "$BASEPATH/action_carry.sh" + + diff --git a/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali.sh b/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali.sh new file mode 100755 index 0000000..5a64185 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +BASEPATH=$(cd `dirname $0`; pwd) +gnome-terminal -x bash -c "$BASEPATH/send_topic.sh" + + diff --git a/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali_auto.sh b/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali_auto.sh new file mode 100755 index 0000000..91eb33a --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/publish_start_to_cali_auto.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +BASEPATH=$(cd `dirname $0`; pwd) +gnome-terminal -x bash -c "$BASEPATH/send_topic_auto.sh" + + diff --git a/src/spark_app/spark_sagittarius_carry/scripts/send_topic.sh b/src/spark_app/spark_sagittarius_carry/scripts/send_topic.sh new file mode 100755 index 0000000..bc79fa1 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/send_topic.sh @@ -0,0 +1,98 @@ +#!/bin/bash + +Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m" +Info="${Green_font_prefix}[信息]${Font_color_suffix}" +echo -e "${Info} 流程如下：" +echo -e "${Info} 1. 获取方块的 hsv" +echo -e "${Info} 1). 机械臂会运行到指定位置，请把方块放置到机械臂夹爪正下方" +echo -e "${Info} 2). 调整摄像头位置，让绿色框对准蓝色方块顶面" +echo -e "${Info} 3). 获取绿色框内 HSV 均值" +echo -e "${Info} 2. 使用九点标定法标定机械臂" +echo -e "${Info} 1). 机械臂会运行到指定位置，请把方块放置到机械臂夹爪正下方" +echo -e "${Info} 2). 程序根据 HSV 获取并保存方块在摄像头上的位置" +echo -e "${Info} 3). 运行下一个标定点，直到九个标定点都完成" +echo -e "${Info} 3. 计算线性回归后的值，并保存到文件" + +echo -e "" +echo -e "${Info} 一切准备好？\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null + +echo -e "" +echo -e "${Info} 请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start2'" -1 >/dev/null + +echo -e "" +echo -e "${Info} 请调整摄像头，把绿色框对准了蓝色方块顶面位置\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start3'" -1 >/dev/null + +sleep 5 +echo -e "" +echo -e "${Info} (1/9) No.01" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (2/9) No.02" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (3/9) No.03" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (4/9) No.04" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (5/9) No.05" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (6/9) No.06" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (7/9) No.07" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (8/9) No.08" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 3 +echo -e "" +echo -e "${Info} (9/9) No.09" +echo -e "${Info} 等待机械臂运动到新位置后，请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c，完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null + +sleep 5 +echo && stty erase '^H' && read -p "标定结束，按任意键退出..." +exit \ No newline at end of file diff --git a/src/spark_app/spark_sagittarius_carry/scripts/send_topic_auto.sh b/src/spark_app/spark_sagittarius_carry/scripts/send_topic_auto.sh new file mode 100755 index 0000000..27e44e8 --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/scripts/send_topic_auto.sh @@ -0,0 +1,36 @@ +#!/bin/bash + +Green_font_prefix="\033[32m" && Red_font_prefix="\033[31m" && Green_background_prefix="\033[42;37m" && Red_background_prefix="\033[41;37m" && Yellow_background_prefix="\033[43;37m" && Font_color_suffix="\033[0m" && Yellow_font_prefix="\e[1;33m" && Blue_font_prefix="\e[0;34m" +Info="${Green_font_prefix}[信息]${Font_color_suffix}" +echo -e "${Info} 流程如下：" +echo -e "${Info} 1. 获取方块的 hsv" +echo -e "${Info} 1). 机械臂会运行到指定位置, 请把方块放置到机械臂夹爪正下方" +echo -e "${Info} 2). 调整摄像头位置, 让绿色框对准蓝色方块顶面" +echo -e "${Info} 3). 程序获取绿色框内 HSV 均值" +echo -e "${Info} 2. 机械臂开始进行标定, 过程自动" +echo -e "${Info} 3. 计算线性回归后的值, 并保存到文件" + +echo -e "" +echo -e "${Info} 一切准备好？\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start'" -1 >/dev/null + +echo -e "" +echo -e "${Info} 请把蓝色方块放到机械臂夹爪的正下方\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..." +rostopic pub /cali_arm_cmd_topic std_msgs/String "data: 'go'" -1 >/dev/null +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start2'" -1 >/dev/null + +echo -e "" +echo -e "${Info} 请调整摄像头, 把绿色框对准了蓝色方块顶面位置\c" +echo && stty erase '^H' && read -p "退出请输入 Ctrl + c, 完成后按回车键继续..." +rostopic pub /cali_cmd_topic std_msgs/String "data: 'start3'" -1 >/dev/null + +sleep 5 +echo -e "" +echo -e "${Info} 机械臂开始自动标定" +echo -e "${Info} 当机械臂停止运动, 而且原来窗口显示完成标定的提示" +echo -e "${Info} 说明标定完成, 可在原来的窗口按 Ctrl-C 退出程序" +echo && stty erase '^H' && read -p "当前窗口可退出, 按回车键退出当前窗口..." +exit \ No newline at end of file diff --git a/src/spark_app/spark_sagittarius_carry/srv/scene.srv b/src/spark_app/spark_sagittarius_carry/srv/scene.srv new file mode 100755 index 0000000..028cd5e --- /dev/null +++ b/src/spark_app/spark_sagittarius_carry/srv/scene.srv @@ -0,0 +1,14 @@ +uint8 END = 0 +uint8 RUN = 1 +uint8 PEND = 2 +uint8 SEARCH = 3 + +uint8 type +string param +--- +uint8 SUCCESS = 0 +uint8 RUNNING = 1 +uint8 PENDING = 2 +uint8 FAILURE = 3 + +uint8 status diff --git a/src/spark_driver/arm/sagittarius_arm_ros/install.sh b/src/spark_driver/arm/sagittarius_arm_ros/install.sh new file mode 100755 index 0000000..0ccc3bb --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/install.sh @@ -0,0 +1,55 @@ + +#检查系统要求 +check_sys(){ + ubuntu_version=$(lsb_release -r --short) + if [[ "${ubuntu_version}" == "16.04" ]]; then + ROS_Ver="kinetic" + elif [[ "${ubuntu_version}" == "18.04" ]]; then + ROS_Ver="melodic" + elif [[ "${ubuntu_version}" == "20.04" ]]; then + ROS_Ver="noetic" + else + echo -e "${Error} SPARK暂不支持当前系统 ${OSDescription} !" && exit 1 + fi +} +BASEPATH=$(cd `dirname $0`; pwd) +echo "sudo cp $BASEPATH/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules /etc/udev/rules.d/" +sudo cp $BASEPATH/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules /etc/udev/rules.d/ +sudo udevadm trigger +check_sys +sudo apt -y install ros-${ROS_Ver}-dynamixel-workbench-toolbox ros-${ROS_Ver}-moveit-msgs ros-${ROS_Ver}-moveit-ros ros-${ROS_Ver}-ompl ros-${ROS_Ver}-moveit-planners-ompl ros-${ROS_Ver}-moveit-simple-controller-manager ros-${ROS_Ver}-joint-state-publisher-gui ros-${ROS_Ver}-moveit-commander ros-${ROS_Ver}-trac-ik-kinematics-plugin ros-${ROS_Ver}-moveit-setup-assistant ros-${ROS_Ver}-moveit-fake-controller-manager ros-${ROS_Ver}-moveit-visual-tools ros-${ROS_Ver}-joy ros-${ROS_Ver}-joint-trajectory-controller ros-${ROS_Ver}-joint-state-controller ros-${ROS_Ver}-gazebo-ros-control ros-${ROS_Ver}-effort-controllers ros-${ROS_Ver}-chomp-motion-planner ros-${ROS_Ver}-moveit-planners-chomp ros-${ROS_Ver}-pilz-industrial-motion-planner + +if [ $(dpkg-query -W -f='${Status}' librealsense2 2>/dev/null | grep -c "ok installed") -eq 0 ]; then + echo "Installing librealsense2..." + sudo apt-key adv --keyserver keys.gnupg.net --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE + sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -sc) main" -u + if [ $ubuntu_version == "16.04" ]; then + version="2.48.0-0~realsense0.4974" + elif [ $ubuntu_version == "18.04" ]; then + version="2.48.0-0~realsense0.4975" + elif [ $ubuntu_version == "20.04" ]; then + version="2.48.0-0~realsense0.4976" + fi + + sudo apt -y install librealsense2-udev-rules=${version} + sudo apt -y install librealsense2-dkms + sudo apt -y install librealsense2=${version} + sudo apt -y install librealsense2-gl=${version} + sudo apt -y install librealsense2-gl-dev=${version} + sudo apt -y install librealsense2-gl-dbg=${version} + sudo apt -y install librealsense2-net=${version} + sudo apt -y install librealsense2-net-dev=${version} + sudo apt -y install librealsense2-net-dbg=${version} + sudo apt -y install librealsense2-utils=${version} + sudo apt -y install librealsense2-dev=${version} + sudo apt -y install librealsense2-dbg=${version} + sudo apt-mark hold librealsense2* + sudo apt -y install ros-${ROS_Ver}-ddynamic-reconfigure +else + echo "librealsense2 already installed!" +fi + +echo "sudo cp $BASEPATH/sdk_sagittarius_arm/rules/usb_cam.rules /etc/udev/rules.d/" +sudo cp $BASEPATH/sdk_sagittarius_arm/rules/usb_cam.rules /etc/udev/rules.d/ +sudo udevadm trigger +check_sys diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/CMakeLists.txt new file mode 100644 index 0000000..afe905c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/CMakeLists.txt @@ -0,0 +1,216 @@ +cmake_minimum_required(VERSION 3.0.2) +project(sagittarius_joy) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## 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 + roscpp + rospy + joy + sdk_sagittarius_arm + sensor_msgs + message_generation + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + arm_joy.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs + std_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + LIBRARIES sagittarius_joy + CATKIN_DEPENDS roscpp rospy joy sdk_sagittarius_arm sensor_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/sagittarius_joy.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/sagittarius_joy_node.cpp) + + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +add_executable(sagittarius_joy src/sagittarius_joy.cpp) +target_link_libraries(sagittarius_joy ${catkin_LIBRARIES}) + + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_sagittarius_joy.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/README.MD b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/README.MD new file mode 100644 index 0000000..7571d1e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/README.MD @@ -0,0 +1,37 @@ +使用方式 + +连接 xbox 或 ps4, 新终端输入下面指令 + +``` +roslaunch sagittarius_joy joy_demo.launch +``` +`controller_type` 是手柄类型，可选为[xbox360, ps4]，默认使用 xbox 手柄 + +然后可以使用手柄控制机械臂 + +控制方式 + +左摇杆前/后: end-effect x 加/减 +左摇杆左/右: 腰部旋转 左摆/右摆 +左摇杆摁下: 无 + +右摇杆前/后: end-effect pitch 往下看/往上看 +右摇杆左/右: end-effect y 加/减 +右摇杆摁下: 特殊设置 摆正夹爪 + +LB(L1): 高度减少 +RB(R1): 高度增加 + +BACK(SHARE): 逆时针翻滚 +START(OPTION): 顺时针翻滚 + +LT(L2): 夹爪减少间距 +RT(R2): 夹爪增加间距 + +方向键上/下: 速度增加/减少 +方向键上/下: 无 + +B(cycle): 预设姿态 home +A(cross): 预设姿态 sleep + +长按 power 1秒: 扭矩输出切换 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy.launch new file mode 100644 index 0000000..3d91914 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy_demo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy_demo.launch new file mode 100644 index 0000000..a2270a6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/launch/joy_demo.launch @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/msg/arm_joy.msg b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/msg/arm_joy.msg new file mode 100644 index 0000000..c73b367 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/msg/arm_joy.msg @@ -0,0 +1,85 @@ + +# This message is used specifically in the sagittarius_joy package +# +# Maps raw 'joy' commands to more specific ones to control an sagittarius + +# enum values that define the joystick controls for the robot + +######################################################################################################### + +# Control the motion of the virtual 'ee_gripper_link' or end effector using the modern_robotics_ik engine +# Position Control +int8 EE_X_INC = 1 +int8 EE_X_DEC = 2 +int8 EE_Y_INC = 3 +int8 EE_Y_DEC = 4 +int8 EE_Z_INC = 5 +int8 EE_Z_DEC = 6 + +# Orientation Control +int8 EE_ROLL_CCW = 7 +int8 EE_ROLL_CW = 8 +int8 EE_PITCH_UP = 9 +int8 EE_PITCH_DOWN = 10 +int8 EE_YAW_LEFT = 11 +int8 EE_YAW_RIGHT = 12 + +######################################################################################################### + +# Control the motion of independent joints on the Arm or send predefined robot poses +# Pose Control +int8 UP_POSE = 13 +int8 HOME_POSE = 14 +int8 SLEEP_POSE = 15 + +# Position and Orientation reset +int8 POSITION_RESET = 16 +int8 ORIENTATION_RESET = 17 + +######################################################################################################### + +# Customize configurations for the Sagittarius Arm +# Inc/Dec Joint speed +int8 SPEED_INC = 18 +int8 SPEED_DEC = 19 + +# Inc/Dec Gripper spacing +int8 GRIPPER_SPACING_INC = 20 +int8 GRIPPER_SPACING_DEC = 21 + +# Torque On/Off all servos +int8 TORQUE_ON = 22 +int8 TORQUE_OFF = 23 + +# Torque Inc/Dec all servos +int8 TORQUE_INC = 24 +int8 TORQUE_DEC = 25 + +######################################################################################################### + +# Other function for the Sagittarius Arm +# Super Key +int8 SUPER_KEY = 26 + +######################################################################################################### + +# Control the motion of the Sagittarius Arm +int8 ee_x_cmd +int8 ee_y_cmd +int8 ee_z_cmd +int8 ee_roll_cmd +int8 ee_pitch_cmd +int8 ee_yaw_cmd + +# Independent Joint/Pose Control +int8 pose_cmd +int8 reset_cmd + +# Misc. Configs +int8 speed_cmd +int8 gripper_spacing_cmd +int8 torque_cmd +int8 torque_lev_cmd + +# Super key +int8 super_cmd diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/package.xml new file mode 100644 index 0000000..b9a5690 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/package.xml @@ -0,0 +1,79 @@ + + + sagittarius_joy + 0.0.0 + The sagittarius_joy package + + + + + spark + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + joy + roscpp + rospy + sdk_sagittarius_arm + sensor_msgs + std_msgs + message_generation + joy + roscpp + rospy + sdk_sagittarius_arm + sensor_msgs + std_msgs + joy + roscpp + rospy + sdk_sagittarius_arm + sensor_msgs + std_msgs + message_generation + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/scripts/sagittarius_arm b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/scripts/sagittarius_arm new file mode 100755 index 0000000..f030089 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/scripts/sagittarius_arm @@ -0,0 +1,482 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- + +import copy +import threading +import rospy +import numpy as np +import math +from sagittarius_joy.msg import arm_joy +import tf.transformations as tf_transformations +import sdk_sagittarius_arm.msg +import std_msgs.msg +import modern_robotics as mr +from sensor_msgs.msg import JointState + + +class AngleManipulation: + + # Inverts a homogeneous transformation matrix + @staticmethod + def transInv(T): + R, p = T[:3, :3], T[:3, 3] + Rt = np.array(R).T + return np.r_[np.c_[Rt, -np.dot(Rt, p)], [[0, 0, 0, 1]]] + + # Calculates 2D Rotation Matrix given a desired yaw angle + @staticmethod + def yawToRotationMatrix(yaw): + R_z = np.array([[math.cos(yaw), -math.sin(yaw)], + [math.sin(yaw), math.cos(yaw)], + ]) + return R_z + + +class SagittariusCtrlTool: + ''' 机械臂简单控制工具箱 + + ''' + _real_joint_degree = [0, 0, 0, 0, 0, 0] + _joint_degree = [0, 0, 0, 0, 0, 0] + lower_joint_limits = [-2, -1.57, -1.48, -2.9, -1.8, -3.1] + upper_joint_limits = [2, 1.4, 1.8, 2.9, 1.6, 3.1] + Slist = np.array([[0.0, 0.0, -1.0, 0.0, 0.045, 0.0], + [0.0, -1.0, 0.0, 0.125, 0.0, -0.045], + [0.0, -1.0, 0.0, 0.304, 0.0, -0.078], + [-1.0, 0.0, 0.0, 0.0, -0.304, -0.0002], + [0.0, -1.0, 0.0, 0.304, 0.0, -0.24685], + [-1.0, 0.0, 0.0, 0.0, -0.304, 0.00045]]).T + M = np.array([[1.0, 0.0, 0.0, 0.363937], + [0.0, 1.0, 0.0, 0.00045], + [0.0, 0.0, 1.0, 0.304], + [0.0, 0.0, 0.0, 1.0]]) + + _time_start = False + _time_last = 0 + + def __init__(self) -> None: + # publish torque command and joint angle to arm + self.torque_pub = rospy.Publisher( + "control_torque", std_msgs.msg.String, queue_size=3) + self.joint_pub = rospy.Publisher( + "joint/commands", sdk_sagittarius_arm.msg.ArmRadControl, queue_size=3) + self.gripper_pub = rospy.Publisher( + "gripper/command", std_msgs.msg.Float64, queue_size=3) + + # subscribe joint states + self.joint_sub = rospy.Subscriber( + 'joint_states', JointState, callback=self.joint_status_callback, queue_size=30) + + # get current posture + self.joint_mutex = threading.Lock() + rospy.sleep(1) + self._joint_degree = copy.deepcopy(self._real_joint_degree) + self.set_gripper(0) + + def get_FK(self, joint_list): + ''' 正向运动学 + + return: 4x4 end-effect 矩阵 + ''' + return mr.FKinSpace(self.M, self.Slist, joint_list) + + def get_IK(self, matrix, custom_guess=None, joint_num=6): + ''' 逆向运动学 + + return: 如果有解, 返回关节角度列表, 否则返回 None + ''' + solution_found = False + default_guess = [[0.0] * 6 for i in range(3)] + default_guess[1][0] = math.radians(-120) + default_guess[2][0] = math.radians(120) + if(custom_guess is None): + initial_guesses = default_guess + else: + initial_guesses = [list(custom_guess)] + default_guess + T_sd = matrix + for i in range(len(initial_guesses)): + if (joint_num == 6): + theta_list, success = mr.IKinSpace( + self.Slist, self.M, T_sd, initial_guesses[i], 0.001, 0.001) + else: + theta_list, success = mr.IKinSpace( + self.Slist[:, :joint_num], self.M, T_sd, initial_guesses[i][:joint_num], 0.001, 0.001) + if (success): + for i in range(len(theta_list)): + if (theta_list[i] <= -math.pi * 2): + theta_list[i] %= -math.pi * 2 + elif (theta_list[i] >= math.pi * 2): + theta_list[i] %= math.pi * 2 + if (round(theta_list[i], 3) < round(self.lower_joint_limits[i], 3)): + theta_list[i] += math.pi * 2 + elif (round(theta_list[i], 3) > round(self.upper_joint_limits[i], 3)): + theta_list[i] -= math.pi * 2 + if not (self.lower_joint_limits[i] <= theta_list[i] <= self.upper_joint_limits[i]): + if (len(initial_guesses) == 4 and i == 0): + rospy.logwarn( + "custom_guess faild, using default guess.") + break + solution_found = True + break + + if (solution_found): + return theta_list.tolist() + else: + return None + + def joint_status_callback(self, msg): + ''' joint states 回调函数 + ''' + with self.joint_mutex: + self._real_joint_degree = list(copy.deepcopy(msg).position[:6]) + + def set_torque(self, enable): + ''' 设置扭矩输出 + + enable: True 为输出扭矩, False 为不输出扭矩 + + ''' + if (enable): + self.torque_pub.publish(std_msgs.msg.String("open")) + else: + self.torque_pub.publish(std_msgs.msg.String("off")) + return True + + def set_gripper(self, line): + ''' 设置夹爪距离 + + line: 夹爪距离, 单位是 m + ''' + self.gripper_pub.publish(std_msgs.msg.Float64(line)) + return True + + def get_single_joint(self, index): + ''' 返回单个关节的角度 + + index: 关节编号 + + return: 关节角度, 单位为弧度 + ''' + return self._joint_degree[index - 1] + + def set_single_joint(self, index, degree): + ''' 设置单个关节角度 + + index: 关节编号 + ''' + self._joint_degree[index] = degree + self.set_all_joint(self._joint_degree) + return True + + def set_all_joint(self, degrees): + ''' 设置所有关节角度 + + degrees: 关节角度的列表 + + return: 设置结果, 成功为True, 失败为False + ''' + for i in range(len(degrees)): + if not (self.lower_joint_limits[i] <= degrees[i] <= self.upper_joint_limits[i]): + return False + self.joint_pub.publish( + sdk_sagittarius_arm.msg.ArmRadControl(rad=degrees)) + self._joint_degree = degrees + return True + + def get_ee_posture(self): + ''' 获取 end-effect 实际的位姿 + + return: 4x4 end-effect 矩阵 + ''' + return self.get_FK(self._real_joint_degree) + + def get_ee_posture_command(self): + ''' 获取 end-effect 最后一次命令的位姿 + + return: 4x4 end-effect 矩阵 + ''' + return self.get_FK(self._joint_degree) + + def set_ee_posture(self, matrix): + ''' 设置 end-effect 的位姿 + + matrix: 4x4 end-effect 矩阵 + return: 设置结果, 成功为True, 失败为False + ''' + theta = self.get_IK(matrix, self._real_joint_degree) + if (theta is not None): + if (self.set_all_joint(theta)): + return True + if (self._time_start == False): + rospy.loginfo("The posture can not found or out of range.") + self._time_last = rospy.Time.now().to_sec() + self._time_start = True + else: + if (rospy.Time.now().to_sec() - self._time_last > 0): + self._time_start = False + return False + + +class SagittariusJoyDemo: + ''' 手柄控制的例程 + + 控制方式 + + 左摇杆前/后: end-effect x 加/减 + 左摇杆左/右：腰部旋转 左摆/右摆 + 左摇杆摁下: 无 + + 右摇杆前/后: end-effect pitch 往下看/往上看 + 右摇杆左/右: end-effect y 加/减 + 右摇杆摁下: 特殊设置 摆正夹爪 + + LB(L1): 高度减少 + RB(R1): 高度增加 + + BACK(SHARE): 逆时针翻滚 + START(OPTION): 顺时针翻滚 + + LT(L2): 夹爪减少间距 + RT(R2): 夹爪增加间距 + + 方向键上/下: 速度增加/减少 + 方向键上/下: 无 + + B(cycle): 预设姿态 home + A(cross): 预设姿态 sleep + + 长按 power 1秒: 扭矩输出切换 + ''' + + def __init__(self): + self.waist_step = 0.01 # 一号舵机步长 + self.rotate_step = 0.01 # 旋转矩阵步长 + self.translate_step = 0.002 # 变换矩阵步长 + self.gripper_pressure_step = 0.001 # 夹爪步长 + self.current_step_scale = 2 # 步长放大倍数 + self.current_torque_status = True # 扭矩输出标志位 + self.current_gripper_spacing = 0 # 夹爪默认值 + self.joy_msg = arm_joy() + self.joy_mutex = threading.Lock() + self.rate = rospy.Rate(15) + self.armbot = SagittariusCtrlTool() + self.T_sy = np.identity(4) # yaw + self.T_yb = np.identity(4) + self.update_T_yb() + rospy.Subscriber("commands/joy_processed", + arm_joy, self.joy_control_cb) + + def update_speed(self, step_scale): + ''' 速度更新 + + 通过更新步长的放大倍速来修改速度 + step_scale: 步长倍速, 默认 2 + ''' + self.current_step_scale = step_scale + rospy.loginfo("current step scale is %.2f." % self.current_step_scale) + + def update_T_yb(self): + ''' 更新 end-effect 位姿 + + 分离姿态 yaw 为 T_sy, IK运算前需点乘 + ''' + T_sb = self.armbot.get_ee_posture_command() + rpy = tf_transformations.euler_from_matrix(T_sb[:3, :3]) + # rpy = ang.rotationMatrixToEulerAngles(T_sb[:3, :3]) + self.T_sy[:2, :2] = AngleManipulation.yawToRotationMatrix(rpy[2]) + self.T_yb = np.dot(AngleManipulation.transInv(self.T_sy), T_sb) + + def update_gripper_spacing(self, gripper_spacing): + ''' 设置夹爪距离 + + gripper_spacing 夹爪距离, 单位为 m + ''' + self.current_gripper_spacing = gripper_spacing + self.armbot.set_gripper(self.current_gripper_spacing) + rospy.loginfo("Gripper spacing is at %.2f." % + (self.current_gripper_spacing)) + + def joy_control_cb(self, msg): + ''' 获取手柄按键的回调函数 + + ''' + with self.joy_mutex: + self.joy_msg = copy.deepcopy(msg) + + # Check the speed_cmd + if (msg.speed_cmd == arm_joy.SPEED_INC and self.current_step_scale < 4): + self.update_speed(self.current_step_scale + 0.1) + elif (msg.speed_cmd == arm_joy.SPEED_DEC and self.current_step_scale > 0.1): + self.update_speed(self.current_step_scale - 0.1) + + # Check the torque_cmd + if (msg.torque_cmd == arm_joy.TORQUE_ON): + self.armbot.set_torque(True) + self.current_torque_status = True + elif (msg.torque_cmd == arm_joy.TORQUE_OFF): + self.armbot.set_torque(False) + self.current_torque_status = False + + # @brief Main control loop to manipulate the arm + def controller(self): + ''' 控制机械臂的函数 + + 主函数循环回调 + ''' + waist_step = self.waist_step * self.current_step_scale + translate_step = self.translate_step * self.current_step_scale + rotate_step = self.rotate_step * self.current_step_scale + + if (self.current_torque_status == False): + return + + with self.joy_mutex: + msg = copy.deepcopy(self.joy_msg) + + # Check the gripper_cmd + if (msg.gripper_spacing_cmd == arm_joy.GRIPPER_SPACING_INC and self.current_gripper_spacing > -0.03): + self.update_gripper_spacing( + self.current_gripper_spacing - self.gripper_pressure_step) + elif (msg.gripper_spacing_cmd == arm_joy.GRIPPER_SPACING_DEC and self.current_gripper_spacing < 0): + self.update_gripper_spacing( + self.current_gripper_spacing + self.gripper_pressure_step) + + # Check the pose_cmd + if (msg.pose_cmd != 0): + if (msg.pose_cmd == arm_joy.HOME_POSE): + self.armbot.set_all_joint([0, 0, 0, 0, 0, 0]) + elif (msg.pose_cmd == arm_joy.SLEEP_POSE): + self.armbot.set_all_joint([0, 1.4, -1.48, 0, -0.4851, 0]) + elif (msg.pose_cmd == arm_joy.UP_POSE): + pass + self.update_T_yb() + return + + # Check the waist_cmd + if (msg.ee_yaw_cmd != 0): + waist_position = self.armbot.get_single_joint(1) + if (msg.ee_yaw_cmd == arm_joy.EE_YAW_LEFT): + waist_position -= waist_step + if (waist_position >= self.armbot.upper_joint_limits[0]): + waist_position = self.armbot.upper_joint_limits[0] + self.armbot.set_single_joint(0, waist_position) + elif (msg.ee_yaw_cmd == arm_joy.EE_YAW_RIGHT): + waist_position += waist_step + if (waist_position <= self.armbot.lower_joint_limits[0]): + waist_position = self.armbot.lower_joint_limits[0] + self.armbot.set_single_joint(0, waist_position) + self.update_T_yb() + + # Check the position and orientation cmd + else: + position_changed = msg.ee_x_cmd + msg.ee_y_cmd + msg.ee_z_cmd + orientation_changed = msg.ee_roll_cmd + msg.ee_pitch_cmd + msg.ee_yaw_cmd + + if (position_changed + orientation_changed == 0 + and msg.reset_cmd != arm_joy.ORIENTATION_RESET + and msg.reset_cmd != arm_joy.POSITION_RESET): + return + + # Copy the most recent T_yb transform into a temporary variable + T_yb = np.array(self.T_yb) + + # Is position has changed + if (position_changed): + # check ee_x_cmd + if (msg.ee_x_cmd == arm_joy.EE_X_INC): + T_yb[0, 3] += translate_step + elif (msg.ee_x_cmd == arm_joy.EE_X_DEC): + T_yb[0, 3] -= translate_step + + # check ee_y_cmd + if (msg.ee_y_cmd == arm_joy.EE_Y_INC and T_yb[0, 3] > 0.3): + T_yb[1, 3] += translate_step + elif (msg.ee_y_cmd == arm_joy.EE_Y_DEC and T_yb[0, 3] > 0.3): + T_yb[1, 3] -= translate_step + + # check ee_z_cmd + if (msg.ee_z_cmd == arm_joy.EE_Z_INC): + T_yb[2, 3] += translate_step + elif (msg.ee_z_cmd == arm_joy.EE_Z_DEC): + T_yb[2, 3] -= translate_step + + # Is position need reset + elif (msg.reset_cmd == arm_joy.POSITION_RESET): + return + + # Is orientation has changed + if (orientation_changed != 0): + roll, pitch, yaw = tf_transformations.euler_from_matrix( + T_yb[:3, :3]) + + # check ee_roll_cmd + if (msg.ee_roll_cmd == arm_joy.EE_ROLL_CW): + roll += rotate_step + elif (msg.ee_roll_cmd == arm_joy.EE_ROLL_CCW): + roll -= rotate_step + + # check ee_pitch_cmd + if (msg.ee_pitch_cmd == arm_joy.EE_PITCH_DOWN): + pitch += rotate_step + elif (msg.ee_pitch_cmd == arm_joy.EE_PITCH_UP): + pitch -= rotate_step + + # check ee_yaw_cmd + # if (msg.ee_yaw_cmd == arm_joy.EE_YAW_LEFT): + # yaw += rotate_step + # elif (msg.ee_yaw_cmd == arm_joy.EE_YAW_RIGHT): + # yaw -= rotate_step + + T_yb[:3, :3] = tf_transformations.euler_matrix(roll, pitch, yaw)[ + :3, :3] + + # Is orientation need reset + elif (msg.reset_cmd == arm_joy.ORIENTATION_RESET): + T_yb[1, 3] = 0 + roll, pitch, yaw = tf_transformations.euler_from_matrix( + T_yb[:3, :3]) + T_yb[:3, :3] = tf_transformations.euler_matrix(0, pitch, 0)[ + :3, :3] + + # Get desired transformation matrix of the end-effector w.r.t. the base frame + T_sd = np.dot(self.T_sy, T_yb) + success = self.armbot.set_ee_posture(T_sd) + if (success): + self.T_yb = np.array(T_yb) + + +if __name__ == '__main__': + rospy.init_node('sagittarius_joy_demo') + arm = SagittariusJoyDemo() + rospy.sleep(5) + + rospy.loginfo("*********** 控制方式 **************") + rospy.loginfo("左摇杆前/后: end-effect x 加/减") + rospy.loginfo("左摇杆左/右：腰部旋转 左摆/右摆") + rospy.loginfo("左摇杆摁下: 无") + rospy.loginfo("") + rospy.loginfo("右摇杆前/后: end-effect pitch 往下看/往上看") + rospy.loginfo("右摇杆左/右: end-effect y 加/减") + rospy.loginfo("右摇杆摁下: 特殊设置 摆正夹爪") + rospy.loginfo("") + rospy.loginfo("LB(L1): 高度减少") + rospy.loginfo("RB(R1): 高度增加") + rospy.loginfo("") + rospy.loginfo("BACK(SHARE): 逆时针翻滚") + rospy.loginfo("START(OPTION): 顺时针翻滚") + rospy.loginfo("") + rospy.loginfo("LT(L2): 夹爪减少间距") + rospy.loginfo("RT(R2): 夹爪增加间距") + rospy.loginfo("") + rospy.loginfo("方向键上/下: 速度增加/减少") + rospy.loginfo("方向键上/下: 无") + rospy.loginfo("") + rospy.loginfo("B(cycle): 预设姿态 home") + rospy.loginfo("A(cross): 预设姿态 sleep") + rospy.loginfo("") + rospy.loginfo("长按 power 1秒: 扭矩输出切换") + rospy.loginfo("*********************************") + + while not rospy.is_shutdown(): + arm.controller() + arm.rate.sleep() diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/src/sagittarius_joy.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/src/sagittarius_joy.cpp new file mode 100644 index 0000000..60f0572 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_joy/src/sagittarius_joy.cpp @@ -0,0 +1,192 @@ +#include +#include +#include "sagittarius_joy/arm_joy.h" + +// Xbox 360 Controller button mappings +static const std::map xbox360 = {{"SLEEP_POSE", 0}, // buttons start here + {"HOME_POSE", 1}, + {"SUPER_KEY", 2}, + {"UP_POSE", 3}, + {"EE_Z_DEC", 4}, + {"EE_Z_INC", 5}, + {"EE_ROLL_CCW", 6}, + {"EE_ROLL_CW", 7}, + {"TORQUE_ONOFF", 8}, + {"POSITION_RESET", 9}, + {"ORIENTATION_RESET", 10}, + {"EE_YAW", 0}, // axes start here + {"EE_X", 1}, + {"GRIPPER_SPACING_INC", 2}, + {"EE_Y", 3}, + {"EE_PITCH", 4}, + {"GRIPPER_SPACING_DEC", 5}, + {"TORQUE", 6}, + {"SPEED", 7}}; + +// PS4 Controller button mappings +static const std::map ps4 = {{"SLEEP_POSE", 0}, // buttons start here + {"HOME_POSE", 1}, + {"UP_POSE", 2}, + {"SUPER_KEY", 3}, + {"EE_Z_DEC", 4}, + {"EE_Z_INC", 5}, + {"null_1", 6}, + {"null_2", 7}, + {"EE_ROLL_CCW", 8}, + {"EE_ROLL_CW", 9}, + {"TORQUE_ONOFF", 10}, + {"POSITION_RESET", 11}, + {"ORIENTATION_RESET", 12}, + {"EE_YAW", 0}, // axes start here + {"EE_X", 1}, + {"GRIPPER_SPACING_INC", 2}, + {"EE_Y", 3}, + {"EE_PITCH", 4}, + {"GRIPPER_SPACING_DEC", 5}, + {"TORQUE", 6}, + {"SPEED", 7}}; + +ros::Publisher pub_joy_cmd; // ROS Publisher to publish ArmJoy messages +sagittarius_joy::arm_joy prev_joy_cmd; // Keep track of the previously commanded ArmJoy message so that only unique messages are published +std::map cntlr; // Holds the controller button mappings +std::string controller_type; // Holds the name of the controller received from the ROS Parameter server +double threshold; // Joystick sensitivity threshold + +/// @brief Joystick callback to create custom ArmJoy messages to control the Arm +/// @param msg - raw sensor_msgs::Joy data +void joy_state_cb(const sensor_msgs::Joy &msg) +{ + static double time_start; + static bool timer_started = false; + static bool torque_cmd_last_state = true; + sagittarius_joy::arm_joy joy_cmd; + + // Check the ee_x_cmd + if (msg.axes.at(cntlr["EE_X"]) >= threshold) + joy_cmd.ee_x_cmd = sagittarius_joy::arm_joy::EE_X_INC; + else if (msg.axes.at(cntlr["EE_X"]) <= -threshold) + joy_cmd.ee_x_cmd = sagittarius_joy::arm_joy::EE_X_DEC; + + // Check the ee_y_cmd + if (msg.axes.at(cntlr["EE_Y"]) >= threshold) + joy_cmd.ee_y_cmd = sagittarius_joy::arm_joy::EE_Y_INC; + else if (msg.axes.at(cntlr["EE_Y"]) <= -threshold) + joy_cmd.ee_y_cmd = sagittarius_joy::arm_joy::EE_Y_DEC; + + // Check the ee_z_cmd + if (msg.buttons.at(cntlr["EE_Z_INC"]) == 1) + joy_cmd.ee_z_cmd = sagittarius_joy::arm_joy::EE_Z_INC; + else if (msg.buttons.at(cntlr["EE_Z_DEC"]) == 1) + joy_cmd.ee_z_cmd = sagittarius_joy::arm_joy::EE_Z_DEC; + + // Check the ee_roll_cmd + if (msg.buttons.at(cntlr["EE_ROLL_CW"]) == 1) + joy_cmd.ee_roll_cmd = sagittarius_joy::arm_joy::EE_ROLL_CW; + else if (msg.buttons.at(cntlr["EE_ROLL_CCW"]) == 1) + joy_cmd.ee_roll_cmd = sagittarius_joy::arm_joy::EE_ROLL_CCW; + + // Check the ee_pitch_cmd + if (msg.axes.at(cntlr["EE_PITCH"]) >= threshold) + joy_cmd.ee_pitch_cmd = sagittarius_joy::arm_joy::EE_PITCH_DOWN; + else if (msg.axes.at(cntlr["EE_PITCH"]) <= -threshold) + joy_cmd.ee_pitch_cmd = sagittarius_joy::arm_joy::EE_PITCH_UP; + + // Check the ee_yaw_cmd + if (msg.axes.at(cntlr["EE_YAW"]) >= threshold) + joy_cmd.ee_yaw_cmd = sagittarius_joy::arm_joy::EE_YAW_LEFT; + else if (msg.axes.at(cntlr["EE_YAW"]) <= -threshold) + joy_cmd.ee_yaw_cmd = sagittarius_joy::arm_joy::EE_YAW_RIGHT; + + // Check the pose_cmd + if (msg.buttons.at(cntlr["HOME_POSE"]) == 1) + joy_cmd.pose_cmd = sagittarius_joy::arm_joy::HOME_POSE; + else if (msg.buttons.at(cntlr["SLEEP_POSE"]) == 1) + joy_cmd.pose_cmd = sagittarius_joy::arm_joy::SLEEP_POSE; + else if (msg.buttons.at(cntlr["UP_POSE"]) == 1) + joy_cmd.pose_cmd = sagittarius_joy::arm_joy::UP_POSE; + + // Check the reset_cmd + if (msg.buttons.at(cntlr["POSITION_RESET"]) == 1) + joy_cmd.reset_cmd = sagittarius_joy::arm_joy::POSITION_RESET; + else if (msg.buttons.at(cntlr["ORIENTATION_RESET"]) == 1) + joy_cmd.reset_cmd = sagittarius_joy::arm_joy::ORIENTATION_RESET; + + // Check the gripper_cmd + if (msg.axes.at(cntlr["GRIPPER_SPACING_INC"]) <= -0.2) + joy_cmd.gripper_spacing_cmd = sagittarius_joy::arm_joy::GRIPPER_SPACING_INC; + else if (msg.axes.at(cntlr["GRIPPER_SPACING_DEC"]) <= -0.2) + joy_cmd.gripper_spacing_cmd = sagittarius_joy::arm_joy::GRIPPER_SPACING_DEC; + + // Check the speed_cmd + if (msg.axes.at(cntlr["SPEED"]) == 1) + joy_cmd.speed_cmd = sagittarius_joy::arm_joy::SPEED_INC; + else if (msg.axes.at(cntlr["SPEED"]) == -1) + joy_cmd.speed_cmd = sagittarius_joy::arm_joy::SPEED_DEC; + + // Check the torque_cmd + if (msg.buttons.at(cntlr["TORQUE_ONOFF"]) == 1) + { + time_start = ros::Time::now().toSec(); + timer_started = true; + } + else if (msg.buttons.at(cntlr["TORQUE_ONOFF"]) == 0) + { + if (timer_started && ros::Time::now().toSec() - time_start > 0) + if (torque_cmd_last_state == true) + { + joy_cmd.torque_cmd = sagittarius_joy::arm_joy::TORQUE_OFF; + torque_cmd_last_state = false; + } + else + { + joy_cmd.torque_cmd = sagittarius_joy::arm_joy::TORQUE_ON; + torque_cmd_last_state = true; + } + timer_started = false; + } + + // Check the torque_lev_cmd + if (msg.axes.at(cntlr["TORQUE"]) == 1) + joy_cmd.torque_lev_cmd = sagittarius_joy::arm_joy::TORQUE_INC; + else if (msg.axes.at(cntlr["TORQUE"]) == -1) + joy_cmd.torque_lev_cmd = sagittarius_joy::arm_joy::TORQUE_DEC; + + // Check the super_cmd + if (msg.buttons.at(cntlr["SUPER_KEY"]) == 1) + joy_cmd.super_cmd = sagittarius_joy::arm_joy::SUPER_KEY; + + // Only publish a ArmJoy message if any of the following fields have changed. + if (!(prev_joy_cmd.ee_x_cmd == joy_cmd.ee_x_cmd && + prev_joy_cmd.ee_y_cmd == joy_cmd.ee_y_cmd && + prev_joy_cmd.ee_z_cmd == joy_cmd.ee_z_cmd && + prev_joy_cmd.ee_roll_cmd == joy_cmd.ee_roll_cmd && + prev_joy_cmd.ee_pitch_cmd == joy_cmd.ee_pitch_cmd && + prev_joy_cmd.ee_yaw_cmd == joy_cmd.ee_yaw_cmd && + prev_joy_cmd.pose_cmd == joy_cmd.pose_cmd && + prev_joy_cmd.reset_cmd == joy_cmd.reset_cmd && + prev_joy_cmd.speed_cmd == joy_cmd.speed_cmd && + prev_joy_cmd.gripper_spacing_cmd == joy_cmd.gripper_spacing_cmd && + prev_joy_cmd.torque_cmd == joy_cmd.torque_cmd && + prev_joy_cmd.torque_lev_cmd == joy_cmd.torque_lev_cmd && + prev_joy_cmd.super_cmd == joy_cmd.super_cmd)) + pub_joy_cmd.publish(joy_cmd); + prev_joy_cmd = joy_cmd; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "sagittarius_joy"); + ros::NodeHandle n; + ros::param::get("~threshold", threshold); + ros::param::get("~controller", controller_type); + if (controller_type == "xbox360") + cntlr = xbox360; + else if (controller_type == "ps4") + cntlr = ps4; + // else + // cntlr = ps4; + ros::Subscriber sub_joy_raw = n.subscribe("commands/joy_raw", 10, joy_state_cb); + pub_joy_cmd = n.advertise("commands/joy_processed", 10); + ros::spin(); + return 0; +} diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/CMakeLists.txt new file mode 100644 index 0000000..2bdff18 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/CMakeLists.txt @@ -0,0 +1,53 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sagittarius_puppet_control) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## 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 + sdk_sagittarius_arm + rosbag + roscpp + tf2_ros + rviz + sensor_msgs + std_msgs + std_srvs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +catkin_package( + CATKIN_DEPENDS rosbag roscpp tf2_ros rviz sensor_msgs std_msgs std_srvs +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ executable +## Specify libraries to link a library or executable target against +## Add cmake target dependencies of the executable +###add_executable(puppet_control_node src/puppet_control_node.cpp) +###target_link_libraries(puppet_control_node ${catkin_LIBRARIES}) +###add_dependencies(puppet_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(puppet_control_single_node src/puppet_control_single_node.cpp) +target_link_libraries(puppet_control_single_node ${catkin_LIBRARIES}) +add_dependencies(puppet_control_single_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(puppet_control_node src/puppet_control_node.cpp) +target_link_libraries(puppet_control_node ${catkin_LIBRARIES}) +add_dependencies(puppet_control_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/5s.sh b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/5s.sh new file mode 100755 index 0000000..58ea1cf --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/5s.sh @@ -0,0 +1,2 @@ +sleep 5 +rosbag play record1.bag diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/duck_throw.bag b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/duck_throw.bag new file mode 100644 index 0000000..84ac980 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/duck_throw.bag differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record.bag b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record.bag new file mode 100644 index 0000000..7d16626 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record.bag differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record1.bag b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record1.bag new file mode 100644 index 0000000..9286f62 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/record1.bag differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx200_commands.bag b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx200_commands.bag new file mode 100644 index 0000000..89e6423 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx200_commands.bag differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx250s_commands.bag b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx250s_commands.bag new file mode 100644 index 0000000..4b9a783 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/bag/wx250s_commands.bag differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s1.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s1.launch new file mode 100644 index 0000000..f4526a2 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s1.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s2.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s2.launch new file mode 100644 index 0000000..0540c78 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s2.launch @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s3.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s3.launch new file mode 100644 index 0000000..7f1538e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s3.launch @@ -0,0 +1,99 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s4.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s4.launch new file mode 100644 index 0000000..4410b94 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s4.launch @@ -0,0 +1,117 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s5.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s5.launch new file mode 100644 index 0000000..2754ab5 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_s5.launch @@ -0,0 +1,133 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_single.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_single.launch new file mode 100644 index 0000000..0ddee9e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/puppet_control_single.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/rosbag_play.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/rosbag_play.launch new file mode 100644 index 0000000..2b8eac0 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/launch/rosbag_play.launch @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/package.xml new file mode 100644 index 0000000..48906ec --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/package.xml @@ -0,0 +1,43 @@ + + + sagittarius_puppet_control + 0.0.0 + The sagittarius_puppet_control package + help + BSD + + litian.zhuang + + + + catkin + sdk_sagittarius_arm + rosbag + roscpp + tf2_ros + rviz + sensor_msgs + std_msgs + std_srvs + sdk_sagittarius_arm + rosbag + roscpp + tf2_ros + rviz + sensor_msgs + std_msgs + std_srvs + sdk_sagittarius_arm + rosbag + roscpp + tf2_ros + rviz + sensor_msgs + std_msgs + std_srvs + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/rviz/puppet_config.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/rviz/puppet_config.rviz new file mode 100644 index 0000000..6b69fdc --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/rviz/puppet_config.rviz @@ -0,0 +1,375 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /TF1/Frames1 + - /TF1/Tree1 + - /RobotModel1 + - /RobotModel2 + Splitter Ratio: 0.4979674816131592 + Tree Height: 539 + - 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: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 0.25 + 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/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + master/sgr532/ar_tag_link: + Value: true + master/sgr532/base_link: + Value: true + master/sgr532/link1: + Value: true + master/sgr532/link2: + Value: true + master/sgr532/link3: + Value: true + master/sgr532/link4: + Value: true + master/sgr532/link5: + Value: true + master/sgr532/link6: + Value: true + master/sgr532/link_grasping_frame: + Value: true + master/sgr532/link_gripper_left: + Value: true + master/sgr532/link_gripper_right: + Value: true + master/sgr532/usb_cam_link: + Value: true + slaver1/sgr532/ar_tag_link: + Value: true + slaver1/sgr532/base_link: + Value: true + slaver1/sgr532/link1: + Value: true + slaver1/sgr532/link2: + Value: true + slaver1/sgr532/link3: + Value: true + slaver1/sgr532/link4: + Value: true + slaver1/sgr532/link5: + Value: true + slaver1/sgr532/link6: + Value: true + slaver1/sgr532/link_grasping_frame: + Value: true + slaver1/sgr532/link_gripper_left: + Value: true + slaver1/sgr532/link_gripper_right: + Value: true + slaver1/sgr532/usb_cam_link: + Value: true + world: + Value: true + Marker Alpha: 1 + Marker Scale: 0.25 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + master/sgr532/base_link: + master/sgr532/link1: + master/sgr532/link2: + master/sgr532/link3: + master/sgr532/link4: + master/sgr532/link5: + master/sgr532/link6: + master/sgr532/ar_tag_link: + {} + master/sgr532/link_grasping_frame: + {} + master/sgr532/link_gripper_left: + {} + master/sgr532/link_gripper_right: + {} + master/sgr532/usb_cam_link: + {} + slaver1/sgr532/base_link: + slaver1/sgr532/link1: + slaver1/sgr532/link2: + slaver1/sgr532/link3: + slaver1/sgr532/link4: + slaver1/sgr532/link5: + slaver1/sgr532/link6: + slaver1/sgr532/ar_tag_link: + {} + slaver1/sgr532/link_grasping_frame: + {} + slaver1/sgr532/link_gripper_left: + {} + slaver1/sgr532/link_gripper_right: + {} + slaver1/sgr532/usb_cam_link: + {} + Update Interval: 0 + 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 + slaver1/sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + slaver1/sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + slaver1/sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + slaver1/sgr532/usb_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: slaver1/sgr532/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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 + master/sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + master/sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + master/sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + master/sgr532/usb_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: master/sgr532/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 40; 40; 121 + Default Light: true + Fixed Frame: world + 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: 1.5 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5253986120223999 + Target Frame: + Yaw: 0.22039595246315002 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 836 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001ee000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000396000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000364000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1368 + X: 72 + Y: 27 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/scripts/rosbag_play.sh b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/scripts/rosbag_play.sh new file mode 100755 index 0000000..7189bc1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/scripts/rosbag_play.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +BASEPATH=$(cd `dirname $0`; pwd) +echo $BASEPATH +sleep 5 +gnome-terminal -x bash -c "rosbag play $BASEPATH/../bag/record1.bag" + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/src/puppet_control_node.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/src/puppet_control_node.cpp new file mode 100644 index 0000000..d6a8240 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_demo/sagittarius_puppet_control/src/puppet_control_node.cpp @@ -0,0 +1,110 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2022, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include +#include + +#include +#include +#include +#include +#include "sdk_sagittarius_arm/ArmInfo.h" + +sensor_msgs::JointState joint_states; + +/// @brief Joint state callback function +/// @param msg - updated joint states +void joint_state_cb(const sensor_msgs::JointState &msg) +{ + joint_states = msg; +} +#define MAX_SLAVER_ROBOT_NUM 5 +int main(int argc, char **argv) +{ + ros::init(argc, argv, "sagittarius_puppet_control"); + ros::NodeHandle n; + int slaver_robot_num = 1; + int i; + ros::Publisher pub_positions[MAX_SLAVER_ROBOT_NUM]; + ros::Publisher pub_gripper[MAX_SLAVER_ROBOT_NUM]; + std::string robot_name_master, robot_name_slaver1,robot_name_slaver[MAX_SLAVER_ROBOT_NUM]; + ros::param::get("~robot_name_master", robot_name_master); + ros::param::get("~slaver_robot_num", slaver_robot_num); + if(slaver_robot_num > MAX_SLAVER_ROBOT_NUM) + { + ROS_ERROR("arm number must be not more than 5"); + return 1; + } + for(i=0; i(robot_name_slaver[i] + "/joint/commands", 100); + pub_gripper[i] = n.advertise(robot_name_slaver[i] + "/gripper/command", 100); + + } + + // 定阅主机械臂的关节角度 + ros::Subscriber sub_positions = n.subscribe(robot_name_master + "/joint_states", 100, joint_state_cb); + ros::Publisher pub_torque = n.advertise(robot_name_master + "/control_torque", 1); + // 释放主机械臂的锁舵功能 + ros::ServiceClient srv_robot_info = n.serviceClient(robot_name_master + "/get_robot_info"); + ros::Rate loop_rate(100); + bool success; + + // 等待SDK节点开始订阅后，才开始发送。 + while ((pub_positions[slaver_robot_num-1].getNumSubscribers() < 1 || joint_states.position.size() < 1) && ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + while ((pub_torque.getNumSubscribers() < 1) && ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + std_msgs::String msg; + msg.data = "close"; + pub_torque.publish(msg); + + + // 获取机械臂的关节数 + sdk_sagittarius_arm::ArmInfo robot_info_srv; + success = srv_robot_info.call(robot_info_srv); + if (!success) + { + ROS_ERROR("Could not get robot info."); + return 1; + } + + + size_t cntr = 0; + while (ros::ok()) + { + sdk_sagittarius_arm::ArmRadControl pos_msg; + for (size_t i{0}; i < robot_info_srv.response.num_joints; i++) + { + pos_msg.rad.push_back(joint_states.position.at(i)); + } + for(i=0; i + */ +#include +#include "sdk_sagittarius_arm/ArmRadControl.h" +#include "sdk_sagittarius_arm/ArmInfo.h" +#include +#include +#include +#include +#include +sensor_msgs::JointState joint_states; + +/// @brief Joint state callback function +/// @param msg - updated joint states +void joint_state_cb(const sensor_msgs::JointState &msg) +{ + joint_states = msg; +} + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "sagittarius_puppet_control_single"); + ros::NodeHandle n; + + // 定阅主机械臂的关节角度 + ros::Subscriber sub_positions = n.subscribe("joint_states", 100, joint_state_cb); + ros::Publisher pub_positions = n.advertise("joint/commands", 100); + ros::Publisher pub_gripper = n.advertise("gripper/command", 100); + ros::ServiceClient srv_robot_info = n.serviceClient("get_robot_info"); + // 释放主机械臂的锁舵功能 + ros::Publisher pub_torque = n.advertise("control_torque", 1); + ros::Rate loop_rate(100); + bool success; + + // 等待SDK节点开始订阅后，才开始发送。 + while ((pub_positions.getNumSubscribers() < 1 || joint_states.position.size() < 1) && ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + while ((pub_torque.getNumSubscribers() < 1) && ros::ok()) + { + ros::spinOnce(); + loop_rate.sleep(); + } + std_msgs::String msg; + msg.data = "close"; + pub_torque.publish(msg); + + // 获取机械臂的关节数 + sdk_sagittarius_arm::ArmInfo robot_info_srv; + success = srv_robot_info.call(robot_info_srv); + if (!success) + { + ROS_ERROR("Could not get robot info."); + return 1; + } + + size_t cntr = 0; + while (ros::ok()) + { + sdk_sagittarius_arm::ArmRadControl pos_msg; + for (size_t i{0}; i < robot_info_srv.response.num_joints; i++) + { + pos_msg.rad.push_back(joint_states.position.at(i)); + } + pub_positions.publish(pos_msg); + + if (cntr == 10) + { + std_msgs::Float64 gpr_msg; + gpr_msg.data = joint_states.position.at(joint_states.position.size()-1); + pub_gripper.publish(gpr_msg); + cntr = 0; + } + cntr++; + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/CMakeLists.txt new file mode 100644 index 0000000..c2bf415 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(sagittarius_descriptions) + +find_package(catkin REQUIRED) + +catkin_package() + +find_package(roslaunch) + +foreach(dir config launch meshes urdf) + install(DIRECTORY ${dir}/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) +endforeach(dir) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/config/joint_names_sagittarius_descriptions.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/config/joint_names_sagittarius_descriptions.yaml new file mode 100644 index 0000000..586a247 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/config/joint_names_sagittarius_descriptions.yaml @@ -0,0 +1 @@ +controller_joint_names: ['', 'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint_gripper_left', 'joint_gripper_right', ] diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/description.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/description.launch new file mode 100644 index 0000000..b16935a --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/description.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + [sagittarius_joint_states] + + + + [sagittarius_joint_states] + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/display.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/display.launch new file mode 100644 index 0000000..36bc5b6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/display.launch @@ -0,0 +1,20 @@ + + + + + + + \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/gazebo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/gazebo.launch new file mode 100644 index 0000000..d05a730 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/launch/gazebo.launch @@ -0,0 +1,20 @@ + + + + + + \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link1.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link1.STL new file mode 100644 index 0000000..0458969 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link1.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link2.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link2.STL new file mode 100644 index 0000000..e14cd1f Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link2.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link3.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link3.STL new file mode 100644 index 0000000..a30a162 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link3.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link4.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link4.STL new file mode 100644 index 0000000..2a172bd Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link4.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link5.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link5.STL new file mode 100644 index 0000000..2d88cd3 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link5.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link6.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link6.STL new file mode 100644 index 0000000..dbc305b Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link6.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame.STL new file mode 100644 index 0000000..d739375 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame_1.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame_1.STL new file mode 100644 index 0000000..d739375 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_grasping_frame_1.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_left.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_left.STL new file mode 100644 index 0000000..fd07499 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_left.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_right.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_right.STL new file mode 100644 index 0000000..294a6d9 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/link_gripper_right.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/sagittarius_base_link.STL b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/sagittarius_base_link.STL new file mode 100644 index 0000000..324199f Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/meshes/sagittarius_base_link.STL differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/package.xml new file mode 100644 index 0000000..5dc9c7b --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/package.xml @@ -0,0 +1,21 @@ + + sagittarius_descriptions + 1.0.0 + +

URDF Description package for sagittarius_descriptions

+

This package contains configuration data, 3D models and launch files +for sagittarius_descriptions robot

+ + litian.zhuang + + BSD + catkin + roslaunch + robot_state_publisher + rviz + joint_state_publisher_gui + gazebo + + + + \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/rviz/description.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/rviz/description.rviz new file mode 100644 index 0000000..1fc3398 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/rviz/description.rviz @@ -0,0 +1,192 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /RobotModel1/Status1 + Splitter Ratio: 0.5 + Tree Height: 539 + - 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: "" +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: world + 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 + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: /sgr532/robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: sgr532/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: 2.4508087635040283 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.2503983974456787 + Target Frame: + Yaw: 1.577213168144226 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 836 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002a6fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002a6000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005580000003efc0100000002fb0000000800540069006d0065010000000000000558000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002e7000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1368 + X: 72 + Y: 27 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf.rviz new file mode 100644 index 0000000..3643488 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf.rviz @@ -0,0 +1,180 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 557 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.588679016 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +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.0299999993 + 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 + - 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 + link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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 + Topic: /initialpose + - 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.0599999987 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.0500000007 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.00999999978 + Pitch: 0.785398006 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000171000002bffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006200fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002b000002bf000000da00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002bffc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002b000002bf000000b200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000035200fffffffb0000000800540069006d0065010000000000000450000000000000000000000339000002bf00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1200 + X: 73 + Y: 60 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sagittarius_descriptions.csv b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sagittarius_descriptions.csv new file mode 100644 index 0000000..dc36c5a --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sagittarius_descriptions.csv @@ -0,0 +1,11 @@ +Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity +sagittarius_base_link,0.00793179544277737,2.48115460577673E-19,0.0312047945759253,0,0,0,0.940146705073746,0.000950195835062344,3.14825974312405E-20,-0.000200082473207336,0.00267650790326047,-1.01412991222915E-19,0.002902663900239,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/sagittarius_base_link.STL,1,1,1,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/sagittarius_base_link.STL,,A_ASM_5_ASM^sagittarius_descriptions-1/1_5^A_ASM_5_ASM_sagittarius_descriptions-1,坐标系9,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,, +link1,-7.74579608786574E-10,3.23354786979222E-12,0.0344870497564283,0,0,0,0.123745079127031,8.15779591743416E-05,-4.25325094802094E-15,-2.22201794150093E-12,6.7607001964135E-05,-1.59127137655435E-14,4.41297902395382E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link1.STL,1,1,1,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link1.STL,,1^sagittarius_descriptions-1/2_2^1_sagittarius_descriptions-1,坐标系1,基准轴1,joint1,revolute,0.045,0,0.07,0,0,0,sagittarius_base_link,0,0,-1,0,0,-2,2,,,,,,,, +link2,0.0119015312584793,-1.03639845764692E-08,0.150755903867182,0,0,0,0.156160822791157,0.000429266751802994,-2.27426165787001E-11,-5.58862834004722E-05,0.000444676097748043,-2.41993274420629E-10,7.916162292894E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link2.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link2.STL,,2^sagittarius_descriptions-1,坐标系2,基准轴2,joint2,revolute,0,-0.0001,0.055,0,0,0,link1,0,1,0,0,0,-1.4,1.57,,,,,,,, +link3,0.054357321065004,-2.67256405253992E-08,0.00064594073830132,0,0,0,0.0366487313200473,9.28701176876329E-06,-2.23878504272996E-11,6.34310578103811E-07,2.0727596617309E-05,-1.71647322080315E-11,2.42416271089979E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link3.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link3.STL,,3^sagittarius_descriptions-1,坐标系3,基准轴3,joint3,revolute,0.033,-0.0001,0.179,0,0,0,link2,0,1,0,0,0,-1.8,1.57,,,,,,,, +link4,0.0444029485674779,-0.00527865310158246,-1.43738021485262E-10,0,0,0,0.124950088011432,3.52699895078887E-05,-1.14870103744551E-05,2.45948329713632E-14,0.00010928557067047,-3.42525315676607E-12,0.000121451732690361,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link4.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link4.STL,,4^sagittarius_descriptions-1,坐标系4,基准轴4,joint4,revolute,0.0845,0,0,0,0,0,link3,1,0,0,0,0,-2.62,2.62,,,,,,,, +link5,0.0391172635125616,1.22102848665318E-15,0.011359922796391,0,0,0,0.0564710099602834,1.77667699918498E-05,3.77572698012408E-19,-6.79618041712231E-07,1.89643063051294E-05,1.78381670331667E-19,1.66130972969903E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link5.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link5.STL,,5^sagittarius_descriptions-1,坐标系5,基准轴5,joint5,revolute,0.08435,0.00065,0,0,0,0,link4,0,1,0,0,0,-1.57,1.57,,,,,,,, +link6,0.0330748503556987,-1.47583477394259E-10,0.00874151659437283,0,0,0,0.0790875358325134,4.00727396068743E-05,1.5026227935429E-13,6.77337873337555E-06,4.37063090227749E-05,-1.73263828329941E-13,5.69755186998711E-05,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link6.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link6.STL,,6^sagittarius_descriptions-1,坐标系6,基准轴6,joint6,revolute,0.061387,0,0,0,0,0,link5,-1,0,0,0,0,-3.1,3.1,,,,,,,, +link_grasping_frame,-0.29292863789122,-0.000855248417215765,-0.182862893125548,0,0,0,1.55422651132493,0.0246807389934657,6.12977506319177E-05,-0.0174568851774044,0.0450160124726729,0.000114187269419605,0.0218809326193524,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_grasping_frame.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_grasping_frame.STL,,10^sagittarius_descriptions-1,坐标系10,基准轴10,joint_grasping_frame,fixed,0.0557,0,0,0,0,0,link6,0,0,0,0,0,0,0,,,,,,,, +link_gripper_left,0.0168448613850675,0.00562743947549268,-3.34157867051821E-05,0,0,0,0.0185082696416839,1.65620668352765E-06,3.12039082733024E-07,-1.06123823321138E-09,7.56731403094351E-06,5.54920898904154E-11,6.37610666327876E-06,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_left.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_left.STL,,7^sagittarius_descriptions-1,坐标系7,基准轴7,joint_gripper_left,prismatic,0.0695,0.033,0,0,0,0,link6,0,1,0,0,0,-0.025,0,,,,,,,, +link_gripper_right,0.0168448612901284,-0.00562743946967042,3.34158424380449E-05,0,0,0,0.0185082695670405,1.65620670013429E-06,-3.12039085367371E-07,1.06121397059487E-09,7.56731399585561E-06,5.54960746810554E-11,6.37610660879392E-06,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_right.STL,0.752941176470588,0.752941176470588,0.752941176470588,1,0,0,0,0,0,0,package://sagittarius_descriptions/meshes/link_gripper_right.STL,,8^sagittarius_descriptions-1,坐标系8,基准轴8,joint_gripper_right,prismatic,0.0695,-0.033,0,0,0,0,link6,0,-1,0,0,0,-0.025,0,,,,,,,, diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sgr532.urdf.xacro b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sgr532.urdf.xacro new file mode 100644 index 0000000..2f2432d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/sgr532.urdf.xacro @@ -0,0 +1,672 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + 1 + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/spark_sgr_external.urdf.xacro b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/spark_sgr_external.urdf.xacro new file mode 100644 index 0000000..91e12c4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_descriptions/urdf/spark_sgr_external.urdf.xacro @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/CMakeLists.txt new file mode 100644 index 0000000..e62c9b1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/CMakeLists.txt @@ -0,0 +1,34 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sagittarius_gazebo) + +## 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 + controller_manager + effort_controllers + gazebo_ros + gazebo_ros_control + sagittarius_descriptions + joint_state_controller + joint_trajectory_controller +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +catkin_package( + CATKIN_DEPENDS controller_manager effort_controllers gazebo_ros gazebo_ros_control sagittarius_descriptions joint_state_controller joint_trajectory_controller +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} +) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/chomp_planning.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/chomp_planning.yaml new file mode 100644 index 0000000..75258e5 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.01 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearence: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/controllers/sgr532_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/controllers/sgr532_controllers.yaml new file mode 100644 index 0000000..f0e1a26 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/controllers/sgr532_controllers.yaml @@ -0,0 +1,9 @@ +controller_list: + - name: "sagittarius_arm_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: [joint1, joint2, joint3, joint4, joint5, joint6] + - name: "sagittarius_gripper_controller" + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + joints: [joint_gripper_right, joint_gripper_left] diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/joint_limits/sgr532_joint_limits.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/joint_limits/sgr532_joint_limits.yaml new file mode 100644 index 0000000..61f7d57 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/joint_limits/sgr532_joint_limits.yaml @@ -0,0 +1,53 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint2: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint3: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint4: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint5: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint6: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint_gripper_left: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint_gripper_right: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/kinematics.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/kinematics.yaml new file mode 100644 index 0000000..e5b2f17 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/kinematics.yaml @@ -0,0 +1,23 @@ +# position-only-ik and orientation-only-ik don't work unless position_only_ik +# is set to true for the KDL and trac_ik plugins + +# sagittarius_arm: +# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 +# position_only_ik: true + +sagittarius_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_timeout: 0.005 + solve_type: Speed + position_only_ik: false + +# Position-only-ik and orientation-only-ik work automatically for the LMA plugin + +# sagittarius_arm: +# kinematics_solver: lma_kinematics_plugin/LMAKinematicsPlugin +# kinematics_solver_search_resolution: 0.005 +# kinematics_solver_timeout: 0.005 +# kinematics_solver_attempts: 3 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/ompl_planning.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/ompl_planning.yaml new file mode 100644 index 0000000..b8d66f1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/ompl_planning.yaml @@ -0,0 +1,174 @@ +planner_configs: + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +sagittarius_arm: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo +sagittarius_gripper: + default_planner_config: RRTConnect + planner_configs: + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sagittarius_texture.gazebo b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sagittarius_texture.gazebo new file mode 100644 index 0000000..627ec7e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sagittarius_texture.gazebo @@ -0,0 +1,54 @@ + + + + + + gazebo_ros_control/DefaultRobotHWSim + true + + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + Custom/sagittarius + + + + true + + + true + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sgr532_gazebo_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sgr532_gazebo_controllers.yaml new file mode 100644 index 0000000..1bb2a53 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/sgr532_gazebo_controllers.yaml @@ -0,0 +1,52 @@ +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 100 + +sagittarius_arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + gains: # Required because we're controlling an effort interface + joint1: {p: 500.0, d: 1.0, i: 5.0} + joint2: {p: 800.0, d: 1.0, i: 10.0} + joint3: {p: 800.0, d: 1.0, i: 10.0} + joint4: {p: 200.0, d: 0, i: 0} + joint5: {p: 100.0, d: 1.0, i: 3.0} + joint6: {p: 20.0, d: 0, i: 0} +# joint4: {p: 5.0, d: 0, i: 0} + constraints: + goal_time: 0.2 + joint1: + goal: 0.1 + trajectory: 0.2 + joint2: + goal: 0.2 + trajectory: 0.2 + joint3: + goal: 0.2 + trajectory: 0.2 + joint4: + goal: 0.2 + trajectory: 0.2 + joint5: + goal: 0.2 + trajectory: 0.2 + joint6: + goal: 0.2 + trajectory: 0.2 + +sagittarius_gripper_controller: + type: effort_controllers/JointTrajectoryController + joints: + - joint_gripper_left + - joint_gripper_right + gains: + joint_gripper_left: {p: 100.0, d: 1.0, i: 0.0} + joint_gripper_right: {p: 100.0, d: 1.0, i: 0.0} diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/srdf/sgr532.srdf.xacro b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/srdf/sgr532.srdf.xacro new file mode 100644 index 0000000..cb1ce5d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/config/srdf/sgr532.srdf.xacro @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/images/gazebo_rqt_graph.png b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/images/gazebo_rqt_graph.png new file mode 100644 index 0000000..47ddd83 Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/images/gazebo_rqt_graph.png differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/chomp_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..221f5a0 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/demo_gazebo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/demo_gazebo.launch new file mode 100644 index 0000000..5f68428 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/demo_gazebo.launch @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/gazebo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/gazebo.launch new file mode 100644 index 0000000..71c1a82 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/gazebo.launch @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/joystick_control.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/move_group.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/move_group.launch new file mode 100644 index 0000000..a383e28 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/move_group.launch @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit.rviz new file mode 100644 index 0000000..a03e276 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit.rviz @@ -0,0 +1,322 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.4172767102718353 + Tree Height: 371 + - Class: rviz/Help + Name: Help +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 + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Goal_Tolerance: 0 + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: true + MoveIt_Warehouse_Host: 127.0.0.1 + MoveIt_Warehouse_Port: 33829 + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/ee_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/ee_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/fingers_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/gripper_bar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/gripper_prop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/lower_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/upper_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: true + Show Robot Visual: true + Show Trail: false + State Display Time: REALTIME + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: sagittarius_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + world: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/ee_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/ee_gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/fingers_link: + Alpha: 1 + Show Axes: false + Show Trail: false + wx250s/gripper_bar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/gripper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/gripper_prop_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/left_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/lower_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/right_finger_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/upper_forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wx250s/wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /rviz_visual_tools + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 54; 65; 222 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/XYOrbit + Distance: 1.4294620752334595 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.2570607662200928 + Y: 0.09244829416275024 + Z: -1.1920928955078125e-07 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5453984141349792 + Target Frame: world + Yaw: 0.7703981995582581 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000002ad0000039efc0200000008fb000000100044006900730070006c006100790073010000003d00000204000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a0056006900650077007300000001c9000000b00000000000000000fb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000247000001940000018900fffffffb0000000a0056006900650077007300000002ad000001550000000000000000000004850000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1848 + X: 72 + Y: 27 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit_rviz.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit_rviz.launch new file mode 100644 index 0000000..caf6973 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/moveit_rviz.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/ompl_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..c9c7374 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_context.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_context.launch new file mode 100644 index 0000000..fa3ffc2 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_context.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..adf51d9 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sagittarius_moveit_controller_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sagittarius_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..ed0b981 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sagittarius_moveit_controller_manager.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sensor_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..56f6831 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/sensor_manager.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/trajectory_execution.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..48e191d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/trajectory_execution.launch.xml @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/warehouse_settings.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/scripts/sagittarius_black.material b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/scripts/sagittarius_black.material new file mode 100644 index 0000000..62d2e49 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/scripts/sagittarius_black.material @@ -0,0 +1,14 @@ +material Custom/sagittarius +{ + technique + { + pass + { + texture_unit + { + texture sagittarius_black.png + scale 1 1 + } + } + } +} diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/textures/sagittarius_black.png b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/textures/sagittarius_black.png new file mode 100644 index 0000000..e7443eb Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/media/materials/textures/sagittarius_black.png differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/package.xml new file mode 100644 index 0000000..d66456b --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_gazebo/package.xml @@ -0,0 +1,40 @@ + + + sagittarius_gazebo + 0.0.0 + The sagittarius_gazebo package + litian.zhuang + BSD + + litian.zhuang + + + + catkin + controller_manager + effort_controllers + gazebo_ros + gazebo_ros_control + sagittarius_descriptions + joint_state_controller + joint_trajectory_controller + controller_manager + effort_controllers + gazebo_ros + gazebo_ros_control + sagittarius_descriptions + joint_state_controller + joint_trajectory_controller + controller_manager + effort_controllers + gazebo_ros + gazebo_ros_control + sagittarius_descriptions + joint_state_controller + joint_trajectory_controller + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/.setup_assistant b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/.setup_assistant new file mode 100644 index 0000000..7338aba --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/.setup_assistant @@ -0,0 +1,11 @@ +moveit_setup_assistant_config: + URDF: + package: sagittarius_descriptions + relative_path: urdf/sgr532.urdf.xacro + xacro_args: "" + SRDF: + relative_path: config/sgr532.srdf + CONFIG: + author_name: litian.zhuang + author_email: litian.zhuang@nxrobo.com + generated_timestamp: 1650938167 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/CMakeLists.txt new file mode 100644 index 0000000..5fba3da --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.1.3) +project(sagittarius_moveit) + +find_package(catkin REQUIRED) + +catkin_package() + +install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN "setup_assistant.launch" EXCLUDE) +install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/cartesian_limits.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/chomp_planning.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/chomp_planning.yaml new file mode 100644 index 0000000..eb9c912 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/chomp_planning.yaml @@ -0,0 +1,18 @@ +planning_time_limit: 10.0 +max_iterations: 200 +max_iterations_after_collision_free: 5 +smoothness_cost_weight: 0.1 +obstacle_cost_weight: 1.0 +learning_rate: 0.01 +smoothness_cost_velocity: 0.0 +smoothness_cost_acceleration: 1.0 +smoothness_cost_jerk: 0.0 +ridge_factor: 0.0 +use_pseudo_inverse: false +pseudo_inverse_ridge_factor: 1e-4 +joint_update_limit: 0.1 +collision_clearance: 0.2 +collision_threshold: 0.07 +use_stochastic_descent: true +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/fake_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/fake_controllers.yaml new file mode 100644 index 0000000..e4a8d8d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/fake_controllers.yaml @@ -0,0 +1,20 @@ +controller_list: + - name: fake_sagittarius_arm_controller + type: $(arg fake_execution_type) + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - name: fake_sagittarius_gripper_controller + type: $(arg fake_execution_type) + joints: + - joint_gripper_left + - joint_gripper_right +initial: # Define initial robot poses per group + - group: sagittarius_arm + pose: home + - group: sagittarius_gripper + pose: open \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/gazebo_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/joint_limits.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/joint_limits.yaml new file mode 100644 index 0000000..595e82e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/joint_limits.yaml @@ -0,0 +1,53 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 1 +default_acceleration_scaling_factor: 1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + joint1: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint2: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint3: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint4: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint5: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint6: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint_gripper_left: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 + joint_gripper_right: + has_velocity_limits: true + max_velocity: 3.14 + has_acceleration_limits: true + max_acceleration: 1.57 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/kinematics.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/kinematics.yaml new file mode 100644 index 0000000..c34b8c1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/kinematics.yaml @@ -0,0 +1,4 @@ +sagittarius_arm: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ompl_planning.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ompl_planning.yaml new file mode 100644 index 0000000..f19c346 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ompl_planning.yaml @@ -0,0 +1,186 @@ +planner_configs: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: + type: geometric::SBL + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + EST: + type: geometric::EST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LBKPIECE: + type: geometric::LBKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + BKPIECE: + type: geometric::BKPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + KPIECE: + type: geometric::KPIECE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] + failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 + min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 + RRT: + type: geometric::RRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + RRTConnect: + type: geometric::RRTConnect + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + RRTstar: + type: geometric::RRTstar + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 + TRRT: + type: geometric::TRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 + max_states_failed: 10 # when to start increasing temp. default: 10 + temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 + min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 + init_temperature: 10e-6 # initial temperature. default: 10e-6 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() + PRM: + type: geometric::PRM + max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 + PRMstar: + type: geometric::PRMstar + FMT: + type: geometric::FMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 + nearest_k: 1 # use Knearest strategy. default: 1 + cache_cc: 1 # use collision checking cache. default: 1 + heuristics: 0 # activate cost to go heuristics. default: 0 + extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + BFMT: + type: geometric::BFMT + num_samples: 1000 # number of states that the planner should sample. default: 1000 + radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 + nearest_k: 1 # use the Knearest strategy. default: 1 + balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 + optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 + heuristics: 1 # activates cost to go heuristics. default: 1 + cache_cc: 1 # use the collision checking cache. default: 1 + extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 + PDST: + type: geometric::PDST + STRIDE: + type: geometric::STRIDE + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 + degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 + max_degree: 18 # max degree of a node in the GNAT. default: 12 + min_degree: 12 # min degree of a node in the GNAT. default: 12 + max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 + estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 + min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 + BiTRRT: + type: geometric::BiTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 + init_temperature: 100 # initial temperature. default: 100 + frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf + LBTRRT: + type: geometric::LBTRRT + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + epsilon: 0.4 # optimality approximation factor. default: 0.4 + BiEST: + type: geometric::BiEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + ProjEST: + type: geometric::ProjEST + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 + LazyPRM: + type: geometric::LazyPRM + range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() + LazyPRMstar: + type: geometric::LazyPRMstar + SPARS: + type: geometric::SPARS + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 1000 # maximum consecutive failure limit. default: 1000 + SPARStwo: + type: geometric::SPARStwo + stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 + sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 + dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 + max_failures: 5000 # maximum consecutive failure limit. default: 5000 +sagittarius_arm: + default_planner_config: RRT + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint1,joint2) + longest_valid_segment_fraction: 0.005 +sagittarius_gripper: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + projection_evaluator: joints(joint_gripper_left,joint_gripper_right) + longest_valid_segment_fraction: 0.005 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ros_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ros_controllers.yaml new file mode 100644 index 0000000..6a71862 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/ros_controllers.yaml @@ -0,0 +1,100 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: sagittarius_arm + joint_model_group_pose: home +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint_gripper_left + - joint_gripper_right + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + - name: sagittarius_arm_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - name: sagittarius_gripper_controller + action_ns: follow_joint_trajectory + default: True + type: FollowJointTrajectory + joints: + - joint_gripper_left + - joint_gripper_right +sagittarius_gripper_controller: + type: position_controllers/JointTrajectoryController + joints: + - joint_gripper_left + - joint_gripper_right + gains: + joint_gripper_left: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint_gripper_right: + p: 100 + d: 1 + i: 1 + i_clamp: 1 +sagittarius_arm_controller: + type: position_controllers/JointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + gains: + joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sensors_3d.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sensors_3d.yaml new file mode 100644 index 0000000..51010a3 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sensors_3d.yaml @@ -0,0 +1,2 @@ +sensors: + [] \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sgr532.srdf.xacro b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sgr532.srdf.xacro new file mode 100644 index 0000000..cb1ce5d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/sgr532.srdf.xacro @@ -0,0 +1,84 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/simple_moveit_controllers.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..51e4cb1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/simple_moveit_controllers.yaml @@ -0,0 +1,19 @@ +controller_list: + - name: sagittarius_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - name: sagittarius_gripper_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - joint_gripper_left + - joint_gripper_right \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/stomp_planning.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/stomp_planning.yaml new file mode 100644 index 0000000..5a1fa03 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/config/stomp_planning.yaml @@ -0,0 +1,78 @@ +stomp/sagittarius_arm: + group_name: sagittarius_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/sagittarius_gripper: + group_name: sagittarius_gripper + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/chomp_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..ac3a1b4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/chomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/default_warehouse_db.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/default_warehouse_db.launch new file mode 100644 index 0000000..1308a73 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo.launch new file mode 100644 index 0000000..6bb4bc4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_gazebo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_gazebo.launch new file mode 100644 index 0000000..a9f320c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_true.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_true.launch new file mode 100644 index 0000000..96f8ab0 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/demo_true.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [sagittarius_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/fake_moveit_controller_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/fake_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..0aa4d90 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/fake_moveit_controller_manager.launch.xml @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/gazebo.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/gazebo.launch new file mode 100644 index 0000000..d8deed6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/gazebo.launch @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/joystick_control.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group.launch new file mode 100644 index 0000000..f3cdd4c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group.launch @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group1.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group1.launch new file mode 100644 index 0000000..f3cdd4c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/move_group1.launch @@ -0,0 +1,104 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit.rviz new file mode 100644 index 0000000..c3a33de --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit.rviz @@ -0,0 +1,291 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + Splitter Ratio: 0.5 + Tree Height: 203 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: sagittarius_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: sgr532/base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5 + Target Frame: sgr532/base_link + Yaw: -0.6232355833053589 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 836 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f3000002eafc0200000007fb000000100044006900730070006c006100790073010000003d0000015c000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000019f000001880000017d00ffffff0000035f000002ea00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1368 + X: 72 + Y: 27 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit_rviz.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit_rviz.launch new file mode 100644 index 0000000..bf17c89 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/moveit_rviz.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl-chomp_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl-chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..b078992 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl-chomp_planning_pipeline.launch.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl_planning_pipeline.launch.xml new file mode 100644 index 0000000..b21c00e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ompl_planning_pipeline.launch.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_context.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_context.launch new file mode 100644 index 0000000..ba0ca58 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_context.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_pipeline.launch.xml new file mode 100644 index 0000000..4b4d0d6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/planning_pipeline.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_control_moveit_controller_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_controllers.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_controllers.launch new file mode 100644 index 0000000..f09b455 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/run_benchmark_ompl.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..9805a56 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sagittarius_arm_moveit_controller_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sagittarius_arm_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..c3ebf5b --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sagittarius_arm_moveit_controller_manager.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sensor_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..973bc7c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/setup_assistant.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/setup_assistant.launch new file mode 100644 index 0000000..39f39c4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_in_spark.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_in_spark.launch new file mode 100644 index 0000000..682d996 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_in_spark.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [sagittarius_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_sensor_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_sensor_manager.launch.xml new file mode 100644 index 0000000..5d02698 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/sgr532_moveit_sensor_manager.launch.xml @@ -0,0 +1,3 @@ + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/simple_moveit_controller_manager.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..ec470d3 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/spark_sgr532.rviz b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/spark_sgr532.rviz new file mode 100644 index 0000000..be1879c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/spark_sgr532.rviz @@ -0,0 +1,457 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + Splitter Ratio: 0.5 + Tree Height: 258 + - Class: rviz/Help + Name: Help + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 +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 + - Acceleration_Scaling_Factor: 1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 + Name: MotionPlanning + Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/usb_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false + State Display Time: 0.05 s + Trail Step Size: 1 + Trajectory Topic: move_group/display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: sagittarius_arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 + Planning Scene Topic: move_group/monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels + Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/ar_tag_link: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_grasping_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + sgr532/link_gripper_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/link_gripper_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sgr532/usb_cam_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true + Value: true + Velocity_Scaling_Factor: 1 + - 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 + IMU_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Link Tree Style: Links in Alphabetic Order + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + base_gyro_link: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_bottom_screw_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_color_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_left_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera_right_ir_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + camera_right_ir_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + cliff_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_back_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_back_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_front_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_left_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_rear_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ir_bumper_right_link: + Alpha: 1 + Show Axes: false + Show Trail: false + laser_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + left_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lidar_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + room_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + spark_stack: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Name: RobotModel + Robot Description: /robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.75 + Focal Point: + X: -0.10000000149011612 + Y: 0.25 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.6149996519088745 + Target Frame: sgr532/base_link + Yaw: 2.4299380779266357 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 955 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f300000361fc0200000007fb000000100044006900730070006c006100790073010000003d00000193000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001d6000001c80000017d00ffffff0000035f0000036100000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1368 + X: 72 + Y: 27 diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/stomp_planning_pipeline.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..d31a252 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/trajectory_execution.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/trajectory_execution.launch.xml new file mode 100644 index 0000000..eba351d --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/trajectory_execution.launch.xml @@ -0,0 +1,24 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse.launch b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse_settings.launch.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/package.xml new file mode 100644 index 0000000..66eaf57 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sagittarius_moveit/package.xml @@ -0,0 +1,41 @@ + + + sagittarius_moveit + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the sgr532 with the MoveIt Motion Planning Framework + + litian.zhuang + litian.zhuang + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit/issues + https://github.com/ros-planning/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners_ompl + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + sagittarius_descriptions + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/CMakeLists.txt b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/CMakeLists.txt new file mode 100644 index 0000000..cacf7bf --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/CMakeLists.txt @@ -0,0 +1,265 @@ +cmake_minimum_required(VERSION 2.8.3) +project(sdk_sagittarius_arm) + +## Compile as C++11, supported in ROS Kinetic and newer + add_compile_options(-std=c++11) + +## 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 + roscpp + sensor_msgs + diagnostic_updater + dynamic_reconfigure + actionlib + actionlib_msgs + urdf + ) + +## System dependencies are found with CMake's conventions +find_package(Boost REQUIRED COMPONENTS system) +find_package(PkgConfig REQUIRED) + +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) +find_path(YAML_CPP_INCLUDE_DIR + NAMES yaml_cpp.h + PATHS ${YAML_CPP_INCLUDE_DIRS} +) +find_library(YAML_CPP_LIBRARY + NAMES YAML_CPP + PATHS ${YAML_CPP_LIBRARY_DIRS} +) +link_directories(${YAML_CPP_LIBRARY_DIRS}) + +if(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") +add_definitions(-DHAVE_NEW_YAMLCPP) +endif(NOT ${YAML_CPP_VERSION} VERSION_LESS "0.5") + +generate_dynamic_reconfigure_options(cfg/SDKSagittariusArm.cfg) + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder + add_message_files( + FILES + ArmRadControl.msg + SingleRadControl.msg + ) + + +## Generate services in the 'srv' folder + add_service_files( + FILES + ArmInfo.srv + ServoRtInfo.srv + ) +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here + generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a run_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + DEPENDS Boost libusb + CATKIN_DEPENDS roscpp sensor_msgs diagnostic_updater dynamic_reconfigure + LIBRARIES sdk_sagittarius_arm_lib + INCLUDE_DIRS include) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/sdk_sagittarius_arm.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/sdk_sagittarius_arm_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) +add_library(sdk_sagittarius_arm_lib + src/sdk_sagittarius_arm_common.cpp + src/parser_base.cpp) +add_dependencies(sdk_sagittarius_arm_lib ${PROJECT_NAME}_gencfg ${catkin_EXPORTED_TARGETS}) +target_link_libraries(sdk_sagittarius_arm_lib + ${catkin_LIBRARIES}) + +add_executable(sdk_sagittarius_arm + src/sdk_sagittarius_arm.cpp + src/sdk_sagittarius_arm_real.cpp + src/sdk_sagittarius_arm_common_serial.cpp + src/sdk_sagittarius_arm_parser.cpp) +target_link_libraries(sdk_sagittarius_arm sdk_sagittarius_arm_lib ${catkin_LIBRARIES} ${YAML_CPP_LIBRARIES}) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +install(TARGETS sdk_sagittarius_arm_lib + DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) + +install( + TARGETS + sdk_sagittarius_arm + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +install(FILES include/${PROJECT_NAME}/parser_base.h + include/${PROJECT_NAME}/sdk_sagittarius_arm_common.h + + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install(DIRECTORY launch/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch) + +install(DIRECTORY meshes/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/meshes) + +install(DIRECTORY urdf/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf) + +install(DIRECTORY plugins/ + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/plugins) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_sdk_sagittarius_arm.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/SDKSagittariusArm.cfg b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/SDKSagittariusArm.cfg new file mode 100755 index 0000000..56fc2b7 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/SDKSagittariusArm.cfg @@ -0,0 +1,23 @@ +#! /usr/bin/env python +# +# Copyright (C) 2021, University of Osnabrueck +# All rights reserved. +# +# Created on: 06.06.2021 +# +# Author: litian.zhuang +# + +PACKAGE='sdk_sagittarius_arm' +from dynamic_reconfigure.parameter_generator_catkin import * + +from math import pi + +gen = ParameterGenerator() +# Name Type Reconfiguration level Description Default Min Max +gen.add("example", str_t, 0, "this is an example,type:str_t,double_t,int_t,bool_t", "hello world") +gen.add("min_ang", double_t, 0, "The angle of the first range measurement [rad].", -0.75 * pi, -0.75 * pi, 0.75 * pi) +gen.add("skip", int_t, 0, "The number", 0, 0, 9) +gen.add("debug_mode", bool_t, 0, "Run in debug mode", False) + +exit(gen.generate(PACKAGE, "sdk_sagittarius_arm", "SDKSagittariusArm")) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/sgr532.yaml b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/sgr532.yaml new file mode 100644 index 0000000..1d430d0 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/cfg/sgr532.yaml @@ -0,0 +1,77 @@ + +order: [joint1, joint2, joint3, joint4, joint5, joint6, joint_gripper_left] +sleep: [0, 1.4, -1.47, 0, -0.4851, 0] +singles: [] + +joint1: + ID: 1 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 4096 + Min_Position_Limit: -1145 + Max_Position_Limit: 1145 + Secondary_ID: 255 + +joint2: + ID: 2 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 4096 + Min_Position_Limit: -899 + Max_Position_Limit: 802 + Secondary_ID: 255 + +joint3: + ID: 3 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 1 + Velocity_Limit: 4096 + Min_Position_Limit: -847 + Max_Position_Limit: 1031 + Secondary_ID: 255 + +joint4: + ID: 4 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 1 + Velocity_Limit: 4096 + Min_Position_Limit: -1661 + Max_Position_Limit: 1661 + Secondary_ID: 255 + +joint5: + ID: 5 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 4096 + Min_Position_Limit: -1031 + Max_Position_Limit: 916 + Secondary_ID: 255 + +joint6: + ID: 6 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 4096 + Min_Position_Limit: -1776 + Max_Position_Limit: 1776 + Secondary_ID: 255 + +joint_gripper_left: + ID: 7 + Baud_Rate: 1000000 + Return_Delay_Time: 0 + Drive_Mode: 0 + Velocity_Limit: 4096 + Min_Position_Limit: 588 + Max_Position_Limit: 0 + Secondary_ID: 255 + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/parser_base.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/parser_base.h new file mode 100644 index 0000000..359d2fc --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/parser_base.h @@ -0,0 +1,29 @@ +#ifndef PARSER_BASE__ +#define PARSER_BASE__ + +#include "sdk_sagittarius_arm/SDKSagittariusArmConfig.h" +#include "sensor_msgs/LaserScan.h" + +namespace sdk_sagittarius_arm +{ + enum ExitCode + { + ExitSuccess = 0, + ExitError = 1, + ExitFatal = 2 + }; + + class CParserBase + { + public: + CParserBase(); + virtual ~CParserBase(); + + virtual int Parse(char *data, + size_t data_length, + SDKSagittariusArmConfig &config, + sensor_msgs::LaserScan &msg) = 0; + }; +} /*namespace sdk_sagittarius_arm*/ + +#endif /*PARSER_BASE__*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common.h new file mode 100644 index 0000000..677d1c9 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common.h @@ -0,0 +1,131 @@ +#ifndef SDK_ARM_COMMON_H_ +#define SDK_ARM_COMMON_H_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include "sdk_sagittarius_arm_constants.h" +#include "parser_base.h" + +/*Fixed received buffer size*/ +#define RECV_BUFFER_SIZE 1024 +typedef struct +{ + uint8_t flag; // 控制位。 + uint8_t servo_id; // 舵机ID。 + int16_t speed; // 实时速度 + int16_t payload; // 实时负载 + uint8_t voltage; // 实时电压 + uint16_t current; // 实时电流 +}Str_ServoState; + +namespace sdk_sagittarius_arm +{ + class CSDarmCommon + { + public: + CSDarmCommon(ros::NodeHandle& nh, ros::NodeHandle& pnh); + virtual ~CSDarmCommon(); + virtual int Init(); + int LoopOnce(); + void StartReceiveSerail(); + + void PublishJointStates(unsigned char *buf); + void CheckJointNum(sdk_sagittarius_arm::SDKSagittariusArmConfig &config); + void UpdateConfig(sdk_sagittarius_arm::SDKSagittariusArmConfig &newConfig, uint32_t level = 0); + virtual bool RebootDevice(); + sensor_msgs::JointState joint_states; + Str_ServoState servo_state; + + double GetExpectedFreq() const + { + return dExpectedFreq; + } + /*发送控制所有舵机的角度*/ + /** + * \param [in] (v1, v2, v3, v4, v5, v6) v1~v6：1~6号舵机的弧度 + * \param [out] 是否发送成功 + */ + virtual int SendArmAllServer(float v1, float v2, float v3, float v4, float v5, float v6) = 0; + virtual int SendArmAllServerTime(short difftime, float v1, float v2, float v3, float v4, float v5, float v6)= 0; + virtual int SendArmAllServerCB(float v1, float v2, float v3, float v4, float v5, float v6) = 0; + virtual int SendArmEndAction(unsigned char onoff, short value) = 0; + virtual int SetArmVel(unsigned short vel)=0; + virtual int SetArmAcc(unsigned char acc)=0; + virtual int SetArmTorque(int torque[])=0; + virtual int SendSerialData2Arm(char *buf, int length)=0; + virtual int SendGetServoRealTimeInfo(unsigned char id)=0; + + /*发送舵机的锁与释放命令到机械臂*/ + /** + * \param [in] (onoff) 0：锁住； 1：释放 + * \param [out] 是否发送成功 + */ + virtual int SendArmLockOrFree(unsigned char onoff) = 0; + protected: + virtual int InitDevice() = 0; + virtual int InitArm(); + virtual int StopArmLock(); + virtual int CloseDevice() = 0; + + void LoopRcv(); + virtual unsigned char CheckSum(unsigned char *buf); + + + + + + /*从串口读取数据 */ + /* + * \param [in] receiveBuffer 接收缓冲区. + * \param [in] bufferSize 最大的接收字节 (0 terminated). + * \param [out] length 接收到的数据长度. + * */ + virtual int GetDataGram(unsigned char* receiveBuffer, int bufferSize, int *length) = 0; + + virtual bool DestroyThread(boost::thread **th); + virtual void print_hex(unsigned char *buf, int len); + + protected: + diagnostic_updater::Updater mDiagUpdater; + + private: + ros::NodeHandle mNodeHandler; + ros::NodeHandle mPrivNodeHandler; + ros::Publisher mScanPublisher; + ros::Publisher mDataPublisher; + ros::Publisher motionPlan_pub; + // Parser + CParserBase *mParser; + + bool mPublishData; + double dExpectedFreq; + boost::thread *mThrcv; + unsigned char mStoreBuffer[RECV_BUFFER_SIZE]; + unsigned char mRecvBuffer[RECV_BUFFER_SIZE]; + unsigned char mFrameBuffer[RECV_BUFFER_SIZE]; + int mDataLength; + + // Diagnostics + diagnostic_updater::DiagnosedPublisher* mDiagPublisher; + + // Dynamic Reconfigure + SDKSagittariusArmConfig mConfig; + dynamic_reconfigure::Server mDynaReconfigServer; + }; +} // sdk_sagittarius_arm + +#endif // SDK_ARM_COMMON_H_ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common_serial.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common_serial.h new file mode 100644 index 0000000..35bd275 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_common_serial.h @@ -0,0 +1,52 @@ +#ifndef SDK_ARM_COMMON_SERIAL__ +#define SDK_ARM_COMMON_SERIAL__ + +#include +#include +#include + +#include "sdk_sagittarius_arm_common.h" + +namespace sdk_sagittarius_arm +{ + class CSDarmCommonSerial : public CSDarmCommon + { + public: + CSDarmCommonSerial(const std::string &serialname, const std::string &baudrate, int &timelimit, ros::NodeHandle& nh, ros::NodeHandle& pnh, bool free_torque); + virtual ~CSDarmCommonSerial(); + + protected: + /*Override functions*/ + virtual int InitDevice(); + virtual int CloseDevice(); + + //virtual int SendDeviceReq(const char *req, std::vector *resp); + + virtual int SendArmAllServer(float v1, float v2, float v3, float v4, float v5, float v6); + + virtual int SendArmAllServerTime(short difftime, float v1, float v2, float v3, float v4, float v5, float v6); + virtual int SendArmAllServerCB(float v1, float v2, float v3, float v4, float v5, float v6); + virtual int SendArmLockOrFree(unsigned char onoff); + virtual int SendArmEndAction(unsigned char onoff, short value); + virtual int SetArmVel(unsigned short vel); + virtual int SetArmAcc(unsigned char acc); + virtual int SetArmTorque(int torque[]); + virtual int SendGetServoRealTimeInfo(unsigned char id); + + virtual int GetDataGram(unsigned char *receiveBuffer, int bufferSize, int *length); + + virtual int SendSerialData2Arm(char *buf, int length); + + virtual unsigned char CheckSum(unsigned char *buf); + private: + int mFd; + size_t mBytesReceived; + std::string mSerialName; + int mBaudrate; + int mTimeLimit; + int mExitFreeTorque; + }; + +} /*namespace sdk_sagittarius_arm*/ + +#endif /*SDK_ARM_COMMON_SERIAL__*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_constants.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_constants.h new file mode 100644 index 0000000..02d106f --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_constants.h @@ -0,0 +1,23 @@ +#ifndef SDK_ARM_COMMANDS_ +#define SDK_ARM_COMMANDS_ + + + + + + +#define PI 3.141592653589793 +#define TYPE_REQUEST_MESSAGE 0x01 +#define CMD_CONTROL_END_ACTION 0x03 +#define CMD_CONTROL_ID_DEGREE 0x04 +#define CMD_CONTROL_ALL_DEGREE 0x05 + +#define CMD_CONTROL_LOCK_OR_FREE 0x08 +#define CMD_CONTROL_ALL_DEGREE_CB 0x0B //插补控制命令 +#define CMD_CONTROL_ALL_DEGREE_AND_DIFF_TIME 0x0C +#define CMD_SET_SERVO_VEL 0x0D //设置舵机的插补速度 +#define CMD_SET_SERVO_ACC 0x0E //设置舵机的加速度 +#define CMD_SET_SERVO_TORQUE 0x0F //设置舵机的扭矩厌大小 +#define CMD_GET_SERVO_RT_INFO 0x10 //获取舵机实时信息 + +#endif //SDK_ARM_COMMANDS_ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_parser.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_parser.h new file mode 100644 index 0000000..ed25653 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_parser.h @@ -0,0 +1,18 @@ +#ifndef SDK_ARM_PARSER__ +#define SDK_ARM_PARSER__ + +#include + +namespace sdk_sagittarius_arm +{ + class CSDKarmParser : public CParserBase + { + public: + CSDKarmParser(); + virtual ~CSDKarmParser(); + virtual int Parse(char *data, size_t data_length, SDKSagittariusArmConfig &config, sensor_msgs::LaserScan &msg); + + }; +} /*namespace sdk_sagittarius_arm*/ + +#endif /*SDK_ARM_PARSER__*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_real.h b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_real.h new file mode 100644 index 0000000..f466994 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/include/sdk_sagittarius_arm/sdk_sagittarius_arm_real.h @@ -0,0 +1,96 @@ +#ifndef SDK_SAGITTARIUS_ARM_REAL_ +#define SDK_SAGITTARIUS_ARM_REAL_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "sdk_sagittarius_arm/SingleRadControl.h" +#include +#include +#include +#include + +struct Servo +{ + std::string name; // 舵机所在的关节名 + uint8_t servo_id; // 舵机ID。未启用 +}; + + + +namespace sdk_sagittarius_arm +{ + class SagittariusArmReal + { + public: + SagittariusArmReal(ros::NodeHandle nh, ros::NodeHandle pnh, const std::string robot_name, const std::string robot_model); + virtual ~SagittariusArmReal(); + void JointStatesCb(const sensor_msgs::JointState& cmd_arm); + void arm_joint_trajectory_msg_callback(const trajectory_msgs::JointTrajectory &msg); + void arm_joint_trajectory_action_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal); + void arm_gripper_trajectory_action_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal); + void arm_gripper_trajectory_msg_callback(const trajectory_msgs::JointTrajectory &msg); + void arm_execute_joint_trajectory(const ros::TimerEvent&); + short arm_calculate_gripper_degree_position(const float dist); + void arm_set_gripper_linear_position(const float dist); + void arm_set_single_joint_degree_position(short g_degree); + void arm_execute_gripper_trajectory(const ros::TimerEvent&); + void arm_set_joint_positions(const double joint_positions[], double diff_time); + void arm_write_joint_commands(const sdk_sagittarius_arm::ArmRadControl &msg); + void arm_write_gripper_command(const std_msgs::Float64 &msg); + void ControlTorque(const std_msgs::String::ConstPtr &msg); + void arm_get_servo_configs(void); + bool arm_get_servo_info(sdk_sagittarius_arm::ServoRtInfo::Request & req, sdk_sagittarius_arm::ServoRtInfo::Response & res); + bool arm_get_robot_info(sdk_sagittarius_arm::ArmInfo::Request & req, sdk_sagittarius_arm::ArmInfo::Response & res); + bool GetAndSetServoAcceleration(sdk_sagittarius_arm::CSDarmCommon *pt); + bool GetAndSetServoVelocity(sdk_sagittarius_arm::CSDarmCommon *pt); + bool GetAndSetServoTorque(sdk_sagittarius_arm::CSDarmCommon *pt); + private: + std::vector arm_joints; + bool execute_joint_traj; + bool execute_gripper_traj; + bool torque_status; + size_t joint_num_write; + trajectory_msgs::JointTrajectory jnt_tra_msg,gripper_tra_msg; + actionlib::SimpleActionServer *joint_action_server; + actionlib::SimpleActionServer *gripper_action_server; + sensor_msgs::JointState motionPlan_position; + sdk_sagittarius_arm::CSDarmCommon *pSDKarm; + sdk_sagittarius_arm::CSDarmCommon *pTest; + sdk_sagittarius_arm::CSDKarmParser *pParser; + ros::ServiceServer srv_get_robot_info; // Service to get information about the robot info + ros::ServiceServer srv_get_servo_info; // Service to get information about the servo info + ros::Subscriber sub_js, sub_joint_commands, sub_gripper_command, sub_ct; + float angle[10]; + ros::Timer tmr_joint_traj; + ros::Timer tmr_gripper_traj; + sensor_msgs::JointState joint_states; + double joint_start_time; + bool rviz_control; + bool servo_control_trajectory; + double gripper_start_time; // 爪子控制的开始时间 + std::vector home_positions; + std::vector sleep_positions; + uint8_t *joint_ids_read; + uint8_t *joint_ids_write; + std::vector all_joints; + const std::string robot_name; + const std::string robot_model; + + }; +} // sdk_sagittarius_arm + +#endif // SDK_SAGITTARIUS_ARM_REAL_ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Forward_kinematics.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Forward_kinematics.launch new file mode 100755 index 0000000..a54bcd6 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Forward_kinematics.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Inverse_kinematics.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Inverse_kinematics.launch new file mode 100755 index 0000000..70e9f48 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/Inverse_kinematics.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius.launch new file mode 100644 index 0000000..938e652 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius.launch @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius_pn.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius_pn.launch new file mode 100644 index 0000000..85bab36 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/run_sagittarius_pn.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/rviz_control_sagittarius.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/rviz_control_sagittarius.launch new file mode 100644 index 0000000..dc60195 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/rviz_control_sagittarius.launch @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/sdk_sagittarius_arm.launch b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/sdk_sagittarius_arm.launch new file mode 100644 index 0000000..30a2e59 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/launch/sdk_sagittarius_arm.launch @@ -0,0 +1,78 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/ArmRadControl.msg b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/ArmRadControl.msg new file mode 100644 index 0000000..ff392b5 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/ArmRadControl.msg @@ -0,0 +1,3 @@ +# 目标位置的数组 + +float64[] rad diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/SingleRadControl.msg b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/SingleRadControl.msg new file mode 100644 index 0000000..8d7568e --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/msg/SingleRadControl.msg @@ -0,0 +1,4 @@ +# 单个舵机控制 + +string joint_name +float64 rad diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/package.xml b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/package.xml new file mode 100644 index 0000000..7b2ccd5 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/package.xml @@ -0,0 +1,62 @@ + + + sdk_sagittarius_arm + 0.0.1 + + ROS driver of Sagittarius_arm + + + + + + litian.zhuang + + + + + + TODO + + + + + + + + + + + + + litian.zhuang + + + + + + + + + + + + + catkin + + roscpp + sensor_msgs + diagnostic_updater + dynamic_reconfigure + libusb-1.0-dev + actionlib + actionlib_msgs + + actionlib + actionlib_msgs + roscpp + sensor_msgs + diagnostic_updater + dynamic_reconfigure + libusb-1.0 + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/nxrobo_sagittarius.png b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/nxrobo_sagittarius.png new file mode 100644 index 0000000..b87af7e Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/nxrobo_sagittarius.png differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/rosgraph.png b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/rosgraph.png new file mode 100644 index 0000000..3280b5c Binary files /dev/null and b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/picture/rosgraph.png differ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules new file mode 100644 index 0000000..410b5c4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/sagittarius-usb-serial.rules @@ -0,0 +1,2 @@ +SUBSYSTEM=="tty", ATTRS{idVendor}=="2e88", ATTRS{idProduct}=="4603", SYMLINK+="sagittarius", MODE:="0666",OWNER:="root" + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/usb_cam.rules b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/usb_cam.rules new file mode 100644 index 0000000..44cfe9c --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/rules/usb_cam.rules @@ -0,0 +1,2 @@ +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="0c45", ATTRS{idProduct}=="64ab", SYMLINK+="usb_cam", MODE:="0666", OWNER:="root", ATTR{index}=="0" + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Forward_kinematics.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Forward_kinematics.py new file mode 100755 index 0000000..0f036d8 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Forward_kinematics.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import rospy, sys +import moveit_commander +from moveit_commander import MoveGroupCommander +import math + +if __name__ == "__main__": + moveit_commander.roscpp_initialize(sys.argv) + + #初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('sagittarius_arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + gripper = MoveGroupCommander('sagittarius_gripper') + gripper1 = moveit_commander.MoveGroupCommander('sagittarius_gripper') + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('world') + + # 设置目标位置所使用的参考坐标系 + gripper.set_pose_reference_frame('world') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.0001) + arm.set_goal_orientation_tolerance(0.0001) + + gripper1.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + while not rospy.is_shutdown(): + print("\nset joint 2: 0° -> 45°") + print("set joint 3: 0° -> -45°") + tjoint = [0, math.pi / 4, -math.pi / 4, 0, 0, 0] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + print("\nset joint 4: 0° -> 90°") + print("set joint 6: 0° -> 90°") + tjoint = [0, math.pi / 4, -math.pi / 4, math.pi / 2, 0, math.pi / 2] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + print("\nset joint 4: 90° -> -90°") + print("set joint 6: 90° -> -90°") + tjoint = [0, math.pi / 4, -math.pi / 4, -math.pi / 2, 0, -math.pi / 2] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + print("\nset joint 3: 0° -> 90°") + print("set joint 5: 0° -> -90°") + tjoint = [0, 0, math.pi / 2, 0, -math.pi / 2, 0] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + print("\nset joint 4: 0° -> 45°") + tjoint = [0, 0, math.pi / 2, math.pi / 4, -math.pi / 2, 0] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + print("\nset joint 4: 45° -> -45°") + tjoint = [0, 0, math.pi / 2, -math.pi / 4, -math.pi / 2, 0] + arm.set_joint_value_target(tjoint) + arm.go() + rospy.sleep(0.5) + if rospy.is_shutdown(): + break + print(arm.get_current_pose(end_effector_link).pose) + + break + + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + arm.set_named_target('sleep') + arm.go() + rospy.sleep(1) + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(0.5) + + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Inverse_kinematics.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Inverse_kinematics.py new file mode 100755 index 0000000..d7266ed --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/Inverse_kinematics.py @@ -0,0 +1,127 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +from copy import deepcopy +import rospy +import sys +import moveit_commander +from moveit_commander import MoveGroupCommander +import math + + +def move2pose(times, px, py, pz, ox, oy, oz, ow): + wpose = deepcopy(arm.get_current_pose(end_effector_link).pose) # 拷贝对象 + wpose.position.x = px + wpose.position.y = py + wpose.position.z = pz + wpose.orientation.x = ox + wpose.orientation.y = oy + wpose.orientation.z = oz + wpose.orientation.w = ow + arm.set_pose_target(wpose) # 自由曲线 + arm.go() + rospy.sleep(times) + + +def eular2orientation(pitch, yaw, roll): + ox = math.sin(pitch/2)*math.sin(yaw/2)*math.cos(roll/2) + \ + math.cos(pitch/2)*math.cos(yaw/2)*math.sin(roll/2) + oy = math.sin(pitch/2)*math.cos(yaw/2)*math.cos(roll/2) + \ + math.cos(pitch/2)*math.sin(yaw/2)*math.sin(roll/2) + oz = math.cos(pitch/2)*math.sin(yaw/2)*math.cos(roll/2) - \ + math.sin(pitch/2)*math.cos(yaw/2)*math.sin(roll/2) + ow = math.cos(pitch/2)*math.cos(yaw/2)*math.cos(roll/2) - \ + math.sin(pitch/2)*math.sin(yaw/2)*math.sin(roll/2) + return ox, oy, oz, ow + + +if __name__ == "__main__": + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('sagittarius_arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + gripper = MoveGroupCommander('sagittarius_gripper') + gripper1 = moveit_commander.MoveGroupCommander('sagittarius_gripper') + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(False) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('world') + + # 设置目标位置所使用的参考坐标系 + gripper.set_pose_reference_frame('world') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.0001) + arm.set_goal_orientation_tolerance(0.0001) + + gripper1.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + # 归位 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + # 获取当前位置为起始点 + pose = deepcopy(arm.get_current_pose(end_effector_link).pose) + px = pose.position.x + py = pose.position.y + pz = pose.position.z + ox = pose.orientation.x + oy = pose.orientation.y + oz = pose.orientation.z + ow = pose.orientation.w + while not rospy.is_shutdown(): # 运动到正方形四个点 + px -= 0.1 + move2pose(0.5, px, py, pz, ox, oy, oz, ow) + if rospy.is_shutdown(): + break + + py -= 0.1 + move2pose(0.5, px, py, pz, ox, oy, oz, ow) + if rospy.is_shutdown(): + break + + pz -= 0.1 + move2pose(0.5, px, py, pz, ox, oy, oz, ow) + if rospy.is_shutdown(): + break + + py += 0.20 + move2pose(0.5, px, py, pz, ox, oy, oz, ow) + if rospy.is_shutdown(): + break + + pz += 0.1 + move2pose(0.5, px, py, pz, ox, oy, oz, ow) + if rospy.is_shutdown(): + break + + break + + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + arm.set_named_target('sleep') + arm.go() + rospy.sleep(1) + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(0.5) + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/getpose.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/getpose.py new file mode 100755 index 0000000..bae0d44 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/getpose.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- + +import rospy +import sys +import moveit_commander +from moveit_commander import MoveGroupCommander +from geometry_msgs.msg import Pose +from copy import deepcopy + + +class MoveItCartesianDemo: + def __init__(self): + # + moveit_commander.roscpp_initialize(sys.argv) + + # 初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + gripper = MoveGroupCommander('gripper') + gripper1 = moveit_commander.MoveGroupCommander('gripper') + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('sagittarius_arm_link') + + # 设置目标位置所使用的参考坐标系 + gripper.set_pose_reference_frame('sagittarius_arm_link') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.001) + arm.set_goal_orientation_tolerance(0.001) + + gripper1.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + while not rospy.is_shutdown(): + # 获取当前位姿数据最为机械臂运动的起始位姿 + start_pose = arm.get_current_pose(end_effector_link).pose + print(start_pose) + rospy.sleep(0.1) + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + +if __name__ == "__main__": + try: + MoveItCartesianDemo() + except rospy.ROSInterruptException: + pass diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/moveit_cartesian_demo.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/moveit_cartesian_demo.py new file mode 100755 index 0000000..927fed1 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/moveit_cartesian_demo.py @@ -0,0 +1,147 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- + +import rospy, sys +import moveit_commander +from moveit_commander import MoveGroupCommander +from geometry_msgs.msg import Pose +from copy import deepcopy + +class MoveItCartesianDemo: + def __init__(self): + # + moveit_commander.roscpp_initialize(sys.argv) + + #初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', True) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('arm') + + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('sagittarius_arm_link') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.001) + arm.set_goal_orientation_tolerance(0.001) + + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + + # 控制机械臂先回到初始化位置 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + # 获取当前位姿数据最为机械臂运动的起始位姿 + start_pose = arm.get_current_pose(end_effector_link).pose + + # 初始化路点列表 + waypoints = [] + + # 如果为True,将初始位姿加入路点列表 + if cartesian: + waypoints.append(start_pose) + + # 设置路点数据，并加入路点列表，所有的点都加入 + wpose = deepcopy(start_pose)#拷贝对象 + wpose.position.z -= 0.1 + + if cartesian: #如果设置为True，那么走直线 + waypoints.append(deepcopy(wpose)) + else: #否则就走曲线 + arm.set_pose_target(wpose) #自由曲线 + arm.go() + rospy.sleep(1) + + wpose.position.x += 0.15 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.y += 0.1 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + wpose.position.x -= 0.15 + wpose.position.y -= 0.1 + + if cartesian: + waypoints.append(deepcopy(wpose)) + else: + arm.set_pose_target(wpose) + arm.go() + rospy.sleep(1) + + + #规划过程 + + if cartesian: + fraction = 0.0 #路径规划覆盖率 + maxtries = 100 #最大尝试规划次数 + attempts = 0 #已经尝试规划次数 + + # 设置机器臂当前的状态作为运动初始状态 + arm.set_start_state_to_current_state() + + # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点 + while fraction < 1.0 and attempts < maxtries: + #规划路径 ，fraction返回1代表规划成功 + (plan, fraction) = arm.compute_cartesian_path ( + waypoints, # waypoint poses，路点列表，这里是5个点 + 0.01, # eef_step，终端步进值，每隔0.01m计算一次逆解判断能否可达 + 0.0, # jump_threshold，跳跃阈值，设置为0代表不允许跳跃 + True) # avoid_collisions，避障规划 + + # 尝试次数累加 + attempts += 1 + + # 打印运动规划进程 + if attempts % 10 == 0: + rospy.loginfo("Still trying after " + str(attempts) + " attempts...") + + # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动 + if fraction == 1.0: + rospy.loginfo("Path computed successfully. Moving the arm.") + arm.execute(plan) + rospy.loginfo("Path execution complete.") + # 如果路径规划失败，则打印失败信息 + else: + rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") + + rospy.sleep(1) + + # 控制机械臂先回到初始化位置 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + +if __name__ == "__main__": + try: + MoveItCartesianDemo() + except rospy.ROSInterruptException: + pass + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/precision_test.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/precision_test.py new file mode 100755 index 0000000..b6b279b --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/precision_test.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import rospy, sys +import moveit_commander +from moveit_commander import MoveGroupCommander +from geometry_msgs.msg import Pose +from copy import deepcopy + +class MoveItCartesianDemo: + global arm + global cartesian + global end_effector_link + + def move2pose(self, times, px, py, pz, ox, oy, oz, ow): + global arm + global cartesian + global end_effector_link + # 获取当前位姿数据最为机械臂运动的起始位姿 + start_pose = arm.get_current_pose(end_effector_link).pose + print(start_pose) + # 初始化路点列表 + waypoints = [] + + # 如果为True,将初始位姿加入路点列表 + if cartesian: + waypoints.append(start_pose) + + # 设置路点数据，并加入路点列表，所有的点都加入 + wpose = deepcopy(start_pose)#拷贝对象 + + wpose.position.z -= 0.25 + print("到绿方块的上方") + wpose.position.x = px + wpose.position.y = py + wpose.position.z = pz + wpose.orientation.x = ox + wpose.orientation.y = oy + wpose.orientation.z = oz + wpose.orientation.w = ow + print(wpose) + if cartesian: #如果设置为True，那么走直线 + waypoints.append(deepcopy(wpose)) + else: #否则就走曲线 + arm.set_pose_target(wpose) #自由曲线 + arm.go() + rospy.sleep(times*0.95) + + def __init__(self): + global arm + global cartesian + global end_effector_link + # + moveit_commander.roscpp_initialize(sys.argv) + + #初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + gripper = MoveGroupCommander('gripper') + gripper1 = moveit_commander.MoveGroupCommander('gripper') + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('world') + + # 设置目标位置所使用的参考坐标系 + gripper.set_pose_reference_frame('world') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.001) + arm.set_goal_orientation_tolerance(0.001) + + gripper1.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + # 控制机夹爪打开 + ''' gripper.set_named_target('open') + gripper.go() + rospy.sleep(1) + + # 控制机夹爪关闭 + gripper.set_named_target('close') + gripper.go() + rospy.sleep(1) + print("gripper open") + # 控制机夹爪打开 + gripper1.set_named_target('open') + gripper1.go() + rospy.sleep(3) ''' + # 控制机械臂先回到初始化位置 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + # 夹起来 + print("夹起来") + gripper1.set_joint_value_target([-0.005, -0.005]) + gripper1.go() + rospy.sleep(1) #print("gripper set_joint_value_target([0.1,0.1])") + # 设置夹爪的目标位置，并控制夹爪运动 + #gripper1.set_joint_value_target([-0.01, -0.01]) + #gripper1.go() + #rospy.sleep(1) +# position: +# x: 0.1 +# y: 0.15 +# z: 0.1 +# orientation: +# x: 0 +# y: 0.689296120492 +# z: 0.00956006494475 +# w: 0.723968564058 + while not rospy.is_shutdown(): + if not rospy.is_shutdown(): + self.move2pose(1, 0.15, -0.1, 0.25, 0, 0, 0, 1) + else: + break + if not rospy.is_shutdown(): + self.move2pose(1, 0.25, -0.1, 0.25, 0, 0, 0, 1) + else: + break + + + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + arm.set_named_target('sleep') + arm.go() + rospy.sleep(1) + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + +if __name__ == "__main__": + try: + MoveItCartesianDemo() + except rospy.ROSInterruptException: + pass + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/switchAB.py b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/switchAB.py new file mode 100755 index 0000000..e1e7c67 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/scripts/switchAB.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python +# -*- coding: UTF-8 -*- +import rospy, sys +import moveit_commander +from moveit_commander import MoveGroupCommander +from geometry_msgs.msg import Pose +from copy import deepcopy +import math + +class MoveItCartesianDemo: + global arm + global cartesian + global end_effector_link + + def move2pose_eular(self, times, px, py, pz, pitch, yaw, roll): + global arm + global cartesian + global end_effector_link + # 获取当前位姿数据最为机械臂运动的起始位姿 + start_pose = arm.get_current_pose(end_effector_link).pose + print(start_pose) + # 初始化路点列表 + waypoints = [] + + # 如果为True,将初始位姿加入路点列表 + if cartesian: + waypoints.append(start_pose) + + # 设置路点数据，并加入路点列表，所有的点都加入 + wpose = deepcopy(start_pose)#拷贝对象 + + wpose.position.z -= 0.25 + wpose.position.x = px + wpose.position.y = py + wpose.position.z = pz + wpose.orientation.x=math.sin(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.cos(yaw/2)*math.sin(roll/2) + wpose.orientation.y=math.sin(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)+math.cos(pitch/2)*math.sin(yaw/2)*math.sin(roll/2) + wpose.orientation.z=math.cos(pitch/2)*math.sin(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.cos(yaw/2)*math.sin(roll/2) + wpose.orientation.w=math.cos(pitch/2)*math.cos(yaw/2)*math.cos(roll/2)-math.sin(pitch/2)*math.sin(yaw/2)*math.sin(roll/2) + print(wpose) + if cartesian: #如果设置为True，那么走直线 + waypoints.append(deepcopy(wpose)) + else: #否则就走曲线 + arm.set_pose_target(wpose) #自由曲线 + arm.go() + rospy.sleep(times) + + def __init__(self): + global arm + global cartesian + global end_effector_link + # + moveit_commander.roscpp_initialize(sys.argv) + + #初始化ROS节点 + rospy.init_node('moveit_cartesian_demo', anonymous=True) + + # 是否需要使用笛卡尔空间的运动规划，获取参数，如果没有设置，则默认为True，即走直线 + cartesian = rospy.get_param('~cartesian', False) + + # 初始化需要使用move group控制的机械臂中的arm group + arm = MoveGroupCommander('arm') + + # 初始化需要使用move group控制的机械臂中的gripper group + gripper = MoveGroupCommander('gripper') + gripper1 = moveit_commander.MoveGroupCommander('gripper') + # 当运动规划失败后，允许重新规划 + arm.allow_replanning(True) + + # 设置目标位置所使用的参考坐标系 + arm.set_pose_reference_frame('world') + + # 设置目标位置所使用的参考坐标系 + gripper.set_pose_reference_frame('world') + + # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 + arm.set_goal_position_tolerance(0.001) + arm.set_goal_orientation_tolerance(0.001) + + gripper1.set_goal_joint_tolerance(0.001) + # 设置允许的最大速度和加速度 + arm.set_max_acceleration_scaling_factor(0.5) + arm.set_max_velocity_scaling_factor(0.5) + + # 获取终端link的名称 + end_effector_link = arm.get_end_effector_link() + # 控制机械臂先回到初始化位置 + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + + while not rospy.is_shutdown(): + + # to A + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.05, 0.15, 0.17, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(2) + + # pick up A + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, 0.15, 0.09, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([-0.022, -0.022]) + gripper1.go() + rospy.sleep(2) + + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, 0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # to tmp + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.18, 0, 0.17, math.pi / 2, 0, 0) + else: + break + + # putdown A + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.18, 0, 0.1, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(2) + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.18, 0, 0.17, math.pi / 2, 0, 0) + else: + break + + # to B + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.05, -0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # pick up B + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, -0.15, 0.09, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([-0.022, -0.022]) + gripper1.go() + rospy.sleep(2) + + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, -0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # to A + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.05, 0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # put down B + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, 0.15, 0.1, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(2) + + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, 0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # to tmp + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.18, 0, 0.17, math.pi / 2, 0, 0) + else: + break + + # pick up A + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.18, 0, 0.09, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([-0.022, -0.022]) + gripper1.go() + rospy.sleep(2) + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.18, 0, 0.17, math.pi / 2, 0, 0) + else: + break + + # to B + if not rospy.is_shutdown(): + self.move2pose_eular(2, 0.05, -0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + # put down B + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, -0.15, 0.1, math.pi / 2, 0, 0) + else: + break + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(2) + + if not rospy.is_shutdown(): + self.move2pose_eular(1, 0.05, -0.15, 0.17, math.pi / 2, 0, 0) + else: + break + + arm.set_named_target('home') + arm.go() + rospy.sleep(1) + arm.set_named_target('sleep') + arm.go() + rospy.sleep(1) + gripper1.set_joint_value_target([0, 0]) + gripper1.go() + rospy.sleep(0.5) + # 关闭并退出moveit + moveit_commander.roscpp_shutdown() + moveit_commander.os._exit(0) + + +if __name__ == "__main__": + try: + MoveItCartesianDemo() + except rospy.ROSInterruptException: + pass + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/parser_base.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/parser_base.cpp new file mode 100644 index 0000000..497d656 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/parser_base.cpp @@ -0,0 +1,18 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include "sdk_sagittarius_arm/parser_base.h" + +namespace sdk_sagittarius_arm +{ + CParserBase::CParserBase() + { + } + + CParserBase::~CParserBase() + { + } +} /*namespace sdk_sagittarius_arm*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm.cpp new file mode 100644 index 0000000..0143b8a --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm.cpp @@ -0,0 +1,29 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "sagittarius_arm_node"); + ROS_INFO("sagittarius_arm bring up"); + ros::NodeHandle nh; + ros::NodeHandle pnh("~"); + std::string robot_name, robot_model; + ros::param::get("~robot_name", robot_name); + ros::param::get("~robot_model", robot_model); + sdk_sagittarius_arm::SagittariusArmReal sar(nh, pnh, robot_name, robot_model); + ROS_INFO("Sagittarius driver is running"); + ros::spin(); + + return 0; +} + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common.cpp new file mode 100644 index 0000000..36ffee3 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common.cpp @@ -0,0 +1,272 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +namespace sdk_sagittarius_arm +{ + + CSDarmCommon::CSDarmCommon(ros::NodeHandle& nh, ros::NodeHandle& pnh) : + mThrcv(NULL), + mNodeHandler(nh), + mPrivNodeHandler(pnh) + { + /*Initialize receive buffer*/ + memset(mRecvBuffer, RECV_BUFFER_SIZE, 0); + mDataLength = 0; + + /*Set reconfigure callback*/ + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&sdk_sagittarius_arm::CSDarmCommon::UpdateConfig, this, _1, _2); + mDynaReconfigServer.setCallback(f); + + motionPlan_pub = mNodeHandler.advertise("sagittarius_joint_states",10); + } + + /** + * Ïû»ÙÏß³Ì + */ + bool CSDarmCommon::DestroyThread(boost::thread **th) + { + + if((*th) != NULL) + { + (*th)->interrupt(); + (*th)->join(); + delete (*th); + (*th) = NULL; + return true; + } + return true; + } + + + int CSDarmCommon::StopArmLock() + { + int result = 0; + + return result; + } + + bool CSDarmCommon::RebootDevice() + { + return true; + } + + CSDarmCommon::~CSDarmCommon() + { + DestroyThread(&mThrcv); + printf("sdk_sagittarius_arm drvier exiting.\n"); + } + + int CSDarmCommon::Init() + { + int result = InitDevice(); + if(0 != result) + { + ROS_FATAL("Failed to init device"); + return result; + } + + result = InitArm(); + if(0 != result) + { + ROS_FATAL("Failed to init arm"); + } + + return result; + } + + unsigned char CSDarmCommon::CheckSum(unsigned char *buf) + { + int i; + unsigned char sum = 0; + for(i=0; i=lt) + { + //ROS_WARN("ARM->ROS:[%f,%f,%f,%f,%f,%f]", joint_state.position[0],joint_state.position[1],joint_state.position[2],joint_state.position[3],joint_state.position[4],joint_state.position[5]); + joint_states = joint_state; + motionPlan_pub.publish(joint_state); + lt = ct+100000; + } + } + + + + + int CSDarmCommon::LoopOnce() + { + mDiagUpdater.update(); + int dataLength = 0; + int result = GetDataGram(mRecvBuffer, RECV_BUFFER_SIZE, &dataLength); + if(result != ExitSuccess) + { + /*ROS_ERROR("sdk_sagittarius_arm - Read Error when getting datagram: %d", result); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - Read Error when getting datagram.");*/ + return ExitError; + } + else + { + if(dataLength>0) + { + if((dataLength<255)&&(mDataLength<255)) + { + //print_hex(mRecvBuffer, dataLength); + memcpy(mFrameBuffer+mDataLength, mRecvBuffer, dataLength); + mDataLength = mDataLength+dataLength; + if(mFrameBuffer[0] != 0x55) + mDataLength = 0; + } + else + mDataLength = 0; + if((mDataLength>3)&&(mDataLength >= (mFrameBuffer[2]+5))) //检测包的大小 + { + if((mFrameBuffer[0]==0x55)&&(mFrameBuffer[1]==0xAA)&&(mFrameBuffer[mFrameBuffer[2]+4]==0x7D)&&(CheckSum(mFrameBuffer)==(unsigned char)mFrameBuffer[mFrameBuffer[2]+3])) //校验和与头尾比较 + { + //printf("receive data:"); + //print_hex(mFrameBuffer, mDataLength); + if(mFrameBuffer[4]==0x0A) //升级相关的命令 + { + printf("升级的命令\n"); + } + else if(mFrameBuffer[4]==0x09) //version的命令 + { + if(mFrameBuffer[3]==0x02) + printf("version is %s\n",mFrameBuffer+5); + } + else if(mFrameBuffer[4]==0x06) + { + if(mFrameBuffer[3]==0x01) + { + PublishJointStates(mFrameBuffer+5); + //ROS_INFO("ARM->ROS:length=%d [%.1f,%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", mDataLength, (float(short(mFrameBuffer[5]|(mFrameBuffer[6]<<8))))/10,(float(short(mFrameBuffer[7]|(mFrameBuffer[8]<<8))))/10,(float(short(mFrameBuffer[9]|(mFrameBuffer[10]<<8))))/10,(float(short(mFrameBuffer[11]|(mFrameBuffer[12]<<8))))/10,(float(short(mFrameBuffer[13]|(mFrameBuffer[14]<<8))))/10,(float(short(mFrameBuffer[15]|(mFrameBuffer[16]<<8))))/10,(float(short(mFrameBuffer[17]|(mFrameBuffer[18]<<8))))/10); + + //printf("ARM->ROS: %f,%f,%f,%f,%f,%f\n",float(mFrameBuffer[5]|mFrameBuffer[6]<<8)/10,float(mFrameBuffer[7]|mFrameBuffer[8]<<8)/10,float(mFrameBuffer[9]|mFrameBuffer[10]<<8)/10,float(mFrameBuffer[11]|mFrameBuffer[12]<<8)/10,float(mFrameBuffer[13]|mFrameBuffer[14]<<8)/10,float(mFrameBuffer[15]|mFrameBuffer[16]<<8)/10); + } + } + else if(mFrameBuffer[4] == CMD_GET_SERVO_RT_INFO) //舵机状态反馈 + { + if(mFrameBuffer[3]==0x02) + { + ROS_INFO("servo is respone CMD_GET_SERVO_RT_INFO"); + servo_state.servo_id = mFrameBuffer[5]; + servo_state.speed = mFrameBuffer[6]|(mFrameBuffer[7]<<8); + servo_state.payload = mFrameBuffer[8]|(mFrameBuffer[9]<<8); + servo_state.voltage = mFrameBuffer[10]; + servo_state.current = mFrameBuffer[11]|(mFrameBuffer[12]<<8); + servo_state.flag = 1; + + } + } + else + { + printf("其它的命令\n"); + } + } + else + { + printf("帧数据出错!\n"); + print_hex(mFrameBuffer, mDataLength); + + } + mDataLength = 0; + } + } + } + return ExitSuccess; // return success to continue + } + + void CSDarmCommon::CheckJointNum(sdk_sagittarius_arm::SDKSagittariusArmConfig &config) + { + + } + + void CSDarmCommon::UpdateConfig(sdk_sagittarius_arm::SDKSagittariusArmConfig &newConfig, uint32_t level) + { + CheckJointNum(newConfig); + mConfig = newConfig; + } + +} // sdk_sagittarius_arm diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common_serial.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common_serial.cpp new file mode 100644 index 0000000..770d7ff --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_common_serial.cpp @@ -0,0 +1,552 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include +#include +#include +#include +#include +#include + +namespace sdk_sagittarius_arm +{ + CSDarmCommonSerial::CSDarmCommonSerial(const std::string &serialname, const std::string &baudrate, int &timelimit, ros::NodeHandle& nh, ros::NodeHandle& pnh, bool free_torque): + CSDarmCommon(nh, pnh), + mSerialName(serialname), + mBaudrate(atoi(baudrate.c_str())), + mTimeLimit(timelimit), + mExitFreeTorque(free_torque) + + { + + } + + CSDarmCommonSerial::~CSDarmCommonSerial() + { + if(mExitFreeTorque) + SendArmLockOrFree(0); + CloseDevice(); + } + + int CSDarmCommonSerial::InitDevice() + { + int i; + int speed_arr[] = { B1500000, B1000000, B460800, B230400, B115200, B19200, B9600, B4800, B2400, B1200, B300}; + int name_arr[] = { 1500000, 1000000, 460800, 230400, 115200, 19200, 9600, 4800, 2400, 1200, 300}; + mFd = -1; + mFd = open(mSerialName.c_str(), O_RDWR | O_NOCTTY); + if (mFd == -1) + { + ROS_ERROR("open serial failed :%s fail\n", mSerialName.c_str()); + return ExitError; + } + + struct termios options; + bzero(&options, sizeof(options)); + cfmakeraw(&options);//ÉèÖÃΪRawģʽ + //设置串口输入波特率和输出波特率 + for ( i= 0; i < sizeof(speed_arr) / sizeof(int); i++) + { + if (mBaudrate == name_arr[i]) + { + cfsetispeed(&options, speed_arr[i]); + cfsetospeed(&options, speed_arr[i]); + printf("使用波特率：%d\n",mBaudrate); + break; + } + } + if(i==sizeof(speed_arr) / sizeof(int)) + { + ROS_ERROR("do not support the baudrate：%d\n",mBaudrate); + close(mFd); + return ExitError; + } + + options.c_cflag |= CLOCAL;//¿ØÖÆģʽ£¬±£Ö¤³ÌÐò²»»á³ÉΪ¶Ë¿ÚµÄÕ¼ÓÐÕß + // options.c_cflag &= ~CLOCAL;//¿ØÖÆģʽ£¬±£Ö¤³ÌÐò²»»á³ÉΪ¶Ë¿ÚµÄÕ¼ÓÐÕß + options.c_cflag |= CREAD;//¿ØÖÆģʽ£¬Ê¹Äܶ˿ڶÁÈ¡ÊäÈëµÄÊý¾Ý + options.c_cflag &= ~CRTSCTS;//ÎÞÁ÷¿ØÖÆ + options.c_cflag &= ~CSIZE;//¿ØÖÆģʽ£¬ÆÁ±Î×Ö·û´óСλ + options.c_cflag |= CS8;//8λ + options.c_cflag &= ~PARENB;//ÎÞУÑé + options.c_cflag &= ~CSTOPB;//1ֹͣλ + options.c_oflag &= ~OPOST;//Êä³öģʽ£¬Ô­Ê¼Êý¾ÝÊä³ö + options.c_cc[VMIN] = 0;//¿ØÖÆ×Ö·û, ËùÒª¶ÁÈ¡×Ö·ûµÄ×îСÊýÁ¿ + options.c_cc[VTIME] = 0;//¿ØÖÆ×Ö·û, ¶ÁÈ¡µÚÒ»¸ö×Ö·ûµÄµÈ´ýʱ¼ä£¬µ¥Î»Îª0.1s + + tcflush(mFd, TCIFLUSH);//Òç³öµÄÊý¾Ý¿ÉÒÔ½ÓÊÕ£¬µ«²»¶Á + + //ÉèÖÃÐÂÊôÐÔ£¬TCSANOW£ºËùÓиıäÁ¢¼´ÉúЧ + if (tcsetattr(mFd, TCSANOW, &options) != 0) + { + ROS_ERROR("set device error\n"); + return ExitError; + } + ROS_INFO("open serial: %s successful!\n",mSerialName.c_str()); + return ExitSuccess; + } + + + int CSDarmCommonSerial::CloseDevice() + { + if(mFd!=-1) + { + close(mFd); + mFd = -1; + ROS_ERROR("close serial and Close Device"); + } + } + + + + int CSDarmCommonSerial::SendSerialData2Arm(char *buf, int length) + { + static std::mutex m_mutex; + int n = -1; + if(mFd > 0) + { + m_mutex.lock(); + n = write(mFd, buf, length); + m_mutex.unlock(); + } + return n; + } + + unsigned char CSDarmCommonSerial::CheckSum(unsigned char *buf) + { + int i; + unsigned char sum = 0; + for(i=0; i>8; + buf[8] = CheckSum(buf); //校验和 + buf[9] = 0x7D; //结束位 + hex_printf(buf); + if(SendSerialData2Arm((char *)buf, 10) != 10) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmLockOrFree: Write command failed!"); + + return ExitError; + } + return ExitSuccess; + } + //发送机械臂舵机释放或者锁住命令 + int CSDarmCommonSerial::SendArmLockOrFree(unsigned char onoff) + { + unsigned char buf[30]; + short degree; + if(mFd == -1) + { + /*ROS_ERROR("SendArmLockOrFree: Serial is not open"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmLockOrFree: Serial is not open!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 3; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_CONTROL_LOCK_OR_FREE; //舵机释放or锁住 + buf[5] = onoff; //0:释放； 1：锁住 + buf[6] = CheckSum(buf); //校验和 + buf[7] = 0x7D; //结束位 + + if(SendSerialData2Arm((char *)buf, 8) != 8) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmLockOrFree: Write command failed!"); + + return ExitError; + } + return ExitSuccess; + } + //发送读取舵机实时信息的命令 + int CSDarmCommonSerial::SendGetServoRealTimeInfo(unsigned char id) + { + unsigned char buf[30]; + short degree; + if(mFd == -1) + { + /*ROS_ERROR("SendArmLockOrFree: Serial is not open"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmLockOrFree: Serial is not open!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 3; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_GET_SERVO_RT_INFO; //读取舵机实时信息的命令 + buf[5] = id; //id号 + buf[6] = CheckSum(buf); //校验和 + buf[7] = 0x7D; //结束位 + + if(SendSerialData2Arm((char *)buf, 8) != 8) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendGetServoRealTimeInfo: Write command failed!"); + + return ExitError; + } + return ExitSuccess; + } + //设置舵机的插补速度 + int CSDarmCommonSerial::SetArmVel(unsigned short vel) + { + unsigned char buf[30]; + short degree; + if(mFd == -1) + { + /*mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmVel: Serial is not open!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 4; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_SET_SERVO_VEL; //设置舵机的插补速度 + buf[5] = vel; //速度低位 + buf[6] = vel>>8; //速度高位 + buf[7] = CheckSum(buf); //校验和 + buf[8] = 0x7D; //结束位 + + if(SendSerialData2Arm((char *)buf, 9) != 9) + { + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmVel: Write command failed!"); + return ExitError; + } + ROS_INFO("SetArmVel: %d",vel); + return ExitSuccess; + } + //设置舵机的加速度 + int CSDarmCommonSerial::SetArmAcc(unsigned char acc) + { + unsigned char buf[30]; + if(mFd == -1) + { + /*mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmAcc: Serial is not open!");*/ + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 3; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_SET_SERVO_ACC; //设置舵机的加速度 + buf[5] = acc; //加速度 + buf[6] = CheckSum(buf); //校验和 + buf[7] = 0x7D; //结束位 + + if(SendSerialData2Arm((char *)buf, 8) != 8) + { + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmAcc: Write command failed!"); + return ExitError; + } + ROS_INFO("SetArmAcc: %d",acc); + return ExitSuccess; + } + //设置舵机的扭矩大小 + int CSDarmCommonSerial::SetArmTorque(int torque[]) + { + unsigned char buf[30]; + int i; + if(mFd == -1) + { + /*mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmTorque: Serial is not open!");*/ + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 2*7+2; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_SET_SERVO_TORQUE; //设置舵机的扭矩大小 + for(i=0;i<7;i++) + { + buf[5+2*i] = torque[i]; + buf[6+2*i] = torque[i]>>8; + ROS_INFO("SetArmTorque: %d",torque[i]); + + } + buf[5+2*i] = CheckSum(buf); //校验和 + buf[6+2*i] = 0x7D; //结束位 + + if(SendSerialData2Arm((char *)buf, 7+2*i) != 7+2*i) + { + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SetArmTorque: Write command failed!"); + return ExitError; + } + return ExitSuccess; + } + int CSDarmCommonSerial::SendArmAllServerTime(short difftime, float v1, float v2, float v3, float v4, float v5, float v6) + { + unsigned char buf[30]; + static unsigned char lastbuf[30]; + short degree; + if(mFd == -1) + { + /*ROS_ERROR("SendArmAllServerTime: Serial is not open"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmAllServerTime: Serial is not open!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 16; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_CONTROL_ALL_DEGREE_AND_DIFF_TIME; //多个舵机控制和时间差 + + + buf[5] = difftime; //角度低位 + buf[6] = difftime>>8; + // v1 = -v1; + degree = 1800/PI*v1; //v1转成角度x10 + buf[7] = degree; //角度低位 + buf[8] = degree>>8; //角度高位 + degree = 1800/PI*v2; //v2转成角度x10 + buf[9] = degree; //角度低位 + buf[10] = degree>>8; //角度高位 + degree = 1800/PI*v3; //v3转成角度x10 德晟此处为负 + buf[11] = degree; //角度低位 + buf[12] = degree>>8; //角度高位 + degree = 1800/PI*v4; //v4转成角度x10 + buf[13] = degree; //角度低位 + buf[14] = degree>>8; //角度高位 + degree = 1800/PI*v5; //v5转成角度x10 + buf[15] = degree; //角度低位 + buf[16] = degree>>8; //角度高位 + degree = 1800/PI*v6; //v6转成角度x10 + buf[17] = degree; //角度低位 + buf[18] = degree>>8; //角度高位 + buf[19] = CheckSum(buf); //校验和 + buf[20] = 0x7D; //结束位 + // ROS_INFO("[%d,%d,%d,%d,%d,%d]", short(buf[5]|(buf[6]<<8)),short(buf[7]|(buf[8]<<8)),short(buf[9]|(buf[10]<<8)),short(buf[11]|(buf[12]<<8)),short(buf[13]|(buf[14]<<8)),short(buf[15]|(buf[16]<<8))); + //--- ROS_INFO("time:%dms [%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", difftime, (float(short(buf[7]|(buf[8]<<8))))/10,(float(short(buf[9]|(buf[10]<<8))))/10,(float(short(buf[11]|(buf[12]<<8))))/10,(float(short(buf[13]|(buf[14]<<8))))/10,(float(short(buf[15]|(buf[16]<<8))))/10,(float(short(buf[17]|(buf[18]<<8))))/10); + if(memcmp(buf,lastbuf,19)!=0) + { + memcpy(lastbuf, buf, 21); + if(SendSerialData2Arm((char *)buf, 21) != 21) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendSerialData2Arm: Write command failed!"); + + return ExitError; + } + } + return ExitSuccess; + } + + + //插补控制命令 + int CSDarmCommonSerial::SendArmAllServerCB(float v1, float v2, float v3, float v4, float v5, float v6) + { + unsigned char buf[30]; + static unsigned char lastbuf[30]; + short degree; + if(mFd == -1) + { + /*ROS_ERROR("SendArmAllServerCB: Serial is not open!"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmAllServerCB: Serial is not open!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 14; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_CONTROL_ALL_DEGREE_CB; //插补控制命令 + // v1 = -v1; + degree = 1800/PI*v1; //v1转成角度x10 + buf[5] = degree; //角度低位 + buf[6] = degree>>8; //角度高位 + degree = 1800/PI*v2; //v2转成角度x10 + buf[7] = degree; //角度低位 + buf[8] = degree>>8; //角度高位 + degree = 1800/PI*v3; //v3转成角度x10 德晟此处为负 + buf[9] = degree; //角度低位 + buf[10] = degree>>8; //角度高位 + degree = 1800/PI*v4; //v4转成角度x10 + buf[11] = degree; //角度低位 + buf[12] = degree>>8; //角度高位 + degree = 1800/PI*v5; //v5转成角度x10 + buf[13] = degree; //角度低位 + buf[14] = degree>>8; //角度高位 + degree = 1800/PI*v6; //v6转成角度x10 + buf[15] = degree; //角度低位 + buf[16] = degree>>8; //角度高位 + buf[17] = CheckSum(buf); //校验和 + buf[18] = 0x7D; //结束位 + // ROS_INFO("[%d,%d,%d,%d,%d,%d]", short(buf[5]|(buf[6]<<8)),short(buf[7]|(buf[8]<<8)),short(buf[9]|(buf[10]<<8)),short(buf[11]|(buf[12]<<8)),short(buf[13]|(buf[14]<<8)),short(buf[15]|(buf[16]<<8))); + ROS_INFO("[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", (float(short(buf[5]|(buf[6]<<8))))/10,(float(short(buf[7]|(buf[8]<<8))))/10,(float(short(buf[9]|(buf[10]<<8))))/10,(float(short(buf[11]|(buf[12]<<8))))/10,(float(short(buf[13]|(buf[14]<<8))))/10,(float(short(buf[15]|(buf[16]<<8))))/10); + if(memcmp(buf,lastbuf,19)!=0) + { + memcpy(lastbuf, buf, 19); + if(SendSerialData2Arm((char *)buf, 19) != 19) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendSerialData2Arm: Write command failed!"); + + return ExitError; + } + } + return ExitSuccess; + } + + + int CSDarmCommonSerial::SendArmAllServer(float v1, float v2, float v3, float v4, float v5, float v6) + { + unsigned char buf[30]; + static unsigned char lastbuf[30]; + short degree; + if(mFd == -1) + { + /*ROS_ERROR("SendArmAllServer: Serial is not open!"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendArmAllServer: Serial is not open!!");*/ + + return ExitError; + } + buf[0] = 0x55; //帧头1 55 + buf[1] = 0xAA; //帧头2 AA + buf[2] = 14; //字节数 数据数＋2 + buf[3] = TYPE_REQUEST_MESSAGE; //请求信息 + buf[4] = CMD_CONTROL_ALL_DEGREE; //多个舵机控制 + degree = 1800/PI*v1; //v1转成角度x10 + buf[5] = degree; //角度低位 + buf[6] = degree>>8; //角度高位 + degree = 1800/PI*v2; //v2转成角度x10 + buf[7] = degree; //角度低位 + buf[8] = degree>>8; //角度高位 + degree = -1800/PI*v3; //v3转成角度x10 德晟此处为负 + buf[9] = degree; //角度低位 + buf[10] = degree>>8; //角度高位 + degree = 1800/PI*v4; //v4转成角度x10 + buf[11] = degree; //角度低位 + buf[12] = degree>>8; //角度高位 + degree = 1800/PI*v5; //v5转成角度x10 + buf[13] = degree; //角度低位 + buf[14] = degree>>8; //角度高位 + degree = 1800/PI*v6; //v6转成角度x10 + buf[15] = degree; //角度低位 + buf[16] = degree>>8; //角度高位 + buf[17] = CheckSum(buf); //校验和 + buf[18] = 0x7D; //结束位 + // ROS_INFO("[%d,%d,%d,%d,%d,%d]", short(buf[5]|(buf[6]<<8)),short(buf[7]|(buf[8]<<8)),short(buf[9]|(buf[10]<<8)),short(buf[11]|(buf[12]<<8)),short(buf[13]|(buf[14]<<8)),short(buf[15]|(buf[16]<<8))); + ROS_INFO("[%.1f,%.1f,%.1f,%.1f,%.1f,%.1f]", (float(short(buf[5]|(buf[6]<<8))))/10,(float(short(buf[7]|(buf[8]<<8))))/10,(float(short(buf[9]|(buf[10]<<8))))/10,(float(short(buf[11]|(buf[12]<<8))))/10,(float(short(buf[13]|(buf[14]<<8))))/10,(float(short(buf[15]|(buf[16]<<8))))/10); + if(memcmp(buf,lastbuf,19)!=0) + { + memcpy(lastbuf, buf, 19); + if(SendSerialData2Arm((char *)buf, 19) != 19) + { + ROS_ERROR("Write error for req command"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - SendSerialData2Arm: Write command failed!"); + + return ExitError; + } + } + return ExitSuccess; + } + + + int CSDarmCommonSerial::GetDataGram(unsigned char* receiveBuffer, int bufferSize, int *length) + { + int len,fs_sel; + fd_set fs_read; + int i; + static int first_bit=1; + struct timeval time; + if(mFd == -1) + { + if(first_bit) + { + first_bit = 0; + ROS_ERROR("GetDataGram: Serial is not open"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - GetDataGram: Serial is not open!"); + } + + return ExitError; + } + + if(1)//(!stream_stopped_) + { + FD_ZERO(&fs_read); + FD_SET(mFd,&fs_read); + time.tv_sec = 1;//mTimeLimit; + time.tv_usec = 0; + //使用select实现串口的多路通信 + fs_sel = select(mFd+1,&fs_read,NULL,NULL,&time); + if(fs_sel>0) + { + if(FD_ISSET(mFd,&fs_read)) + { + *length = read(mFd,receiveBuffer,bufferSize); + } + } + else + { + ROS_ERROR_THROTTLE(1.0, "GetDataGram: No full reply available for read after 1s"); + mDiagUpdater.broadcast(diagnostic_msgs::DiagnosticStatus::ERROR, + "sdk_sagittarius_arm - GetDataGram: No full response for read after 5s."); + ROS_DEBUG("GetDataGram timeout for %ds", mTimeLimit); + return ExitError; + } + } + + return ExitSuccess; + } +} /*sdk_sagittarius_arm*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_parser.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_parser.cpp new file mode 100644 index 0000000..23136e8 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_parser.cpp @@ -0,0 +1,29 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include +#include +#include + +namespace sdk_sagittarius_arm +{ + CSDKarmParser::CSDKarmParser() : + CParserBase() + { + // Do Nothing... + } + + CSDKarmParser::~CSDKarmParser() + { + // Do Nothing... + } + + int CSDKarmParser::Parse(char *data, size_t data_length, SDKSagittariusArmConfig &config, sensor_msgs::LaserScan &msg) + { + + return ExitSuccess; + } +} /*namespace sdk_sagittarius_arm*/ diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_real.cpp b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_real.cpp new file mode 100644 index 0000000..303e07a --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/src/sdk_sagittarius_arm_real.cpp @@ -0,0 +1,724 @@ +/* + * Software License Agreement (BSD License) + * Copyright (c) 2021, NXROBO. + * All rights reserved. + * Author: litian.zhuang + */ +#include +namespace sdk_sagittarius_arm +{ + SagittariusArmReal::SagittariusArmReal(ros::NodeHandle nh, ros::NodeHandle pnh, const std::string robot_name, const std::string robot_model) + : robot_name(robot_name), robot_model(robot_model) + { + /*Check whether serialname is provided*/ + bool isSerialConnection = false; + bool exit_free_torque = false; + + std::string strSerialName; + std::string strBaudrate; + bool rviz_coqntrol; + rviz_control = false; + servo_control_trajectory = false; + execute_joint_traj = false; + execute_gripper_traj = false; + torque_status = true; + + if(pnh.getParam("just_rviz_control", rviz_control)) + { + ROS_WARN("rviz_control is %d",rviz_coqntrol); + } + if(pnh.getParam("servo_control_trajectory", servo_control_trajectory)) //直接使用舵机的规划。 + { + ROS_WARN("servo_control_trajectory is %d",servo_control_trajectory); + } + if(pnh.getParam("exit_free_torque", exit_free_torque)) //程序退出时，是否释放舵机扭矩。 + { + ROS_WARN("exit_free_torque is %d",exit_free_torque); + } + + + if(pnh.getParam("serialname", strSerialName)) + { + isSerialConnection = true; + } + if(pnh.getParam("baudrate", strBaudrate)) + { + isSerialConnection = true; + } + /*Get configured time limit*/ + int iTimeLimit = 5; + pnh.param("timelimit", iTimeLimit, 5); + pSDKarm = NULL; + int result = sdk_sagittarius_arm::ExitError; + + arm_get_servo_configs(); + srv_get_robot_info = nh.advertiseService("get_robot_info", &SagittariusArmReal::arm_get_robot_info, this); + + srv_get_servo_info = nh.advertiseService("get_servo_info", &SagittariusArmReal::arm_get_servo_info, this); + +/* pTest = new sdk_sagittarius_arm::CSDarmCommonSerial("/dev/ttyUSB0", "115200", iTimeLimit, nh, pnh); //just for test + result = pTest->Init(); //just for test*/ + pSDKarm = new sdk_sagittarius_arm::CSDarmCommonSerial(strSerialName, strBaudrate, iTimeLimit, nh, pnh, exit_free_torque); + result = pSDKarm->Init(); + sub_ct = nh.subscribe("control_torque", 1, &SagittariusArmReal::ControlTorque, this); + + if(rviz_control) + { + sub_js = nh.subscribe("joint_states", 1, &SagittariusArmReal::JointStatesCb, this); + //motionPlan_pub = nh.advertise("/joint_states",1); + } + else + { + pSDKarm->StartReceiveSerail(); + /*Device has been initliazed successfully*/ + // + + tmr_joint_traj = nh.createTimer(ros::Duration(1/100), &SagittariusArmReal::arm_execute_joint_trajectory, this); + tmr_gripper_traj = nh.createTimer(ros::Duration(1/100), &SagittariusArmReal::arm_execute_gripper_trajectory, this); + // + joint_action_server = new actionlib::SimpleActionServer + (nh, "sagittarius_arm_controller/follow_joint_trajectory", boost::bind(&SagittariusArmReal::arm_joint_trajectory_action_callback, this, _1),false); + + gripper_action_server = new actionlib::SimpleActionServer + (nh, "sagittarius_gripper_controller/follow_joint_trajectory", boost::bind(&SagittariusArmReal::arm_gripper_trajectory_action_callback, this, _1),false); + joint_action_server->start(); + gripper_action_server->start(); + } + GetAndSetServoVelocity(pSDKarm); + sleep(2); + GetAndSetServoAcceleration(pSDKarm); + sleep(2); + GetAndSetServoTorque(pSDKarm); + sleep(2); + + if (!rviz_control) + { + std::string yaml_file; + ros::param::get("~servo_configs", yaml_file); + yaml_file += robot_model + ".yaml"; + YAML::Node sag_config = YAML::LoadFile(yaml_file.c_str()); + if (sag_config.IsNull()) + { + ROS_ERROR("Config file not found."); + } + else + { + sensor_msgs::JointState joint_state; + joint_state = pSDKarm->joint_states; + bool joint_2_3_outrange_flag = false; + double angel_check = 0; + angel_check = sag_config["joint2"]["Max_Position_Limit"].as() / 1800.0 * PI; + if (joint_state.position[1] > angel_check) + { + ROS_WARN("joint2 is out of range."); + joint_state.position[1] = angel_check; + joint_2_3_outrange_flag = true; + } + angel_check = sag_config["joint3"]["Min_Position_Limit"].as() / 1800.0 * PI; + if (pSDKarm->joint_states.position[2] < angel_check) + { + ROS_WARN("joint3 is out of range."); + joint_state.position[2] = angel_check; + joint_2_3_outrange_flag = true; + } + if (joint_2_3_outrange_flag) + pSDKarm->SendArmAllServerCB( + (float)joint_state.position[0], + (float)joint_state.position[1], + (float)joint_state.position[2], + (float)joint_state.position[3], + (float)joint_state.position[4], + (float)joint_state.position[5]); + } + } + + sub_joint_commands = nh.subscribe("joint/commands", 100, &SagittariusArmReal::arm_write_joint_commands, this); + sub_gripper_command = nh.subscribe("gripper/command", 100, &SagittariusArmReal::arm_write_gripper_command, this); + + } + + SagittariusArmReal::~SagittariusArmReal() + { + if(pSDKarm != NULL) + { + delete pSDKarm; + } + + } + + bool SagittariusArmReal::GetAndSetServoVelocity(sdk_sagittarius_arm::CSDarmCommon *pt) + { + std::string str_arm_vel; + int arm_vel; + ros::param::param("~arm_velocity", arm_vel, 1000); + ROS_WARN("arm_vel is %d",arm_vel); + if(pt != NULL) + pt->SetArmVel(arm_vel); + return true; + + } + + bool SagittariusArmReal::GetAndSetServoAcceleration(sdk_sagittarius_arm::CSDarmCommon *pt) + { + std::string str_arm_acc; + int arm_acc; + ros::param::param("~arm_acceleration", arm_acc, 0); + ROS_WARN("arm_acceleration is %d",arm_acc); + if(pt != NULL) + pt->SetArmAcc(arm_acc); + return true; + } + + bool SagittariusArmReal::GetAndSetServoTorque(sdk_sagittarius_arm::CSDarmCommon *pt) + { + std::string str_arm_acc; + int arm_torque[7]; + ros::param::param("~servo_torque1", arm_torque[0], 1000); + ROS_WARN("servo_torque1 is %d",arm_torque[0]); + ros::param::param("~servo_torque2", arm_torque[1], 1000); + ROS_WARN("servo_torque2 is %d",arm_torque[1]); + ros::param::param("~servo_torque3", arm_torque[2], 1000); + ROS_WARN("servo_torque3 is %d",arm_torque[2]); + ros::param::param("~servo_torque4", arm_torque[3], 1000); + ROS_WARN("servo_torque4 is %d",arm_torque[3]); + ros::param::param("~servo_torque5", arm_torque[4], 1000); + ROS_WARN("servo_torque5 is %d",arm_torque[4]); + ros::param::param("~servo_torque6", arm_torque[5], 1000); + ROS_WARN("servo_torque6 is %d",arm_torque[5]); + ros::param::param("~servo_torque7", arm_torque[6], 1000); + ROS_WARN("servo_torque7 is %d",arm_torque[6]); + if(pt != NULL) + pt->SetArmTorque(arm_torque); + return true; + } + + void SagittariusArmReal::arm_get_servo_configs(void) + { + std::string yaml_file; + ros::param::get("~servo_configs", yaml_file); + yaml_file += robot_model + ".yaml"; + YAML::Node sag_config = YAML::LoadFile(yaml_file.c_str()); + if (sag_config.IsNull()) + { + ROS_ERROR("Config file not found."); + return; + } + + // Define the home and sleep positions for the arm joints + YAML::Node sleep_node = sag_config["sleep"]; + for (auto const& value: sleep_node) + { + sleep_positions.push_back(value.as()); + home_positions.push_back(0); + } + YAML::Node order_node = sag_config["order"]; + YAML::Node singles_node = sag_config["singles"]; + std::vector nodes {order_node, singles_node}; + joint_num_write = 0; + if (order_node.size() > 0) + ROS_INFO("size is >0"); + for (auto const& node: nodes) + { + for(size_t i{0}; i < node.size(); i++) + { + std::string name = node[i].as(); + YAML::Node item = sag_config[name]; + int32_t id = item["ID"].as(); + int32_t secondary_id = item["Secondary_ID"].as(); + Servo motor = {name, (uint8_t) id}; + std::vector shadow_list {(uint8_t) id}; + if (node == sag_config["order"] && name != "joint_gripper_left" && secondary_id == 255) + joint_num_write++; + } + } + joint_ids_write = new uint8_t[joint_num_write]; + size_t cntr = 0; + for (auto const& node: nodes) + { + for (size_t i{0}; i < node.size(); i++) + { + std::string name = node[i].as(); + YAML::Node item = sag_config[name]; + int32_t id = item["ID"].as(); + + for (YAML::const_iterator it_item = item.begin(); it_item != item.end(); it_item++) + { + std::string item_name = it_item->first.as(); + int32_t value = it_item->second.as(); + + if (item_name == "ID") + continue; + else if (item_name == "Secondary_ID" && value == 255) + { + Servo joint = {name, (uint8_t) id}; + all_joints.push_back(joint); + + if (node == sag_config["order"]) + { + arm_joints.push_back(joint); + if (name != "joint_gripper_left") + { + joint_ids_write[cntr] = (uint8_t) id; + cntr++; + } + } + } + } + } + } + } + + bool SagittariusArmReal::arm_get_servo_info(sdk_sagittarius_arm::ServoRtInfo::Request & req, sdk_sagittarius_arm::ServoRtInfo::Response & res) + { + int t_cnt = 500; + if (req.servo_id>0) + { + pSDKarm->servo_state.flag = 0; + pSDKarm->SendGetServoRealTimeInfo(req.servo_id); + } + else + { + ROS_ERROR("the servo id must be more than 0"); + return false; + } + while((pSDKarm->servo_state.flag == 0)&&(t_cnt--)) + { + usleep(1000*2); + } + if(pSDKarm->servo_state.flag) + { + res.speed = pSDKarm->servo_state.speed; + res.voltage = pSDKarm->servo_state.voltage; + res.current = pSDKarm->servo_state.current; + res.payload = pSDKarm->servo_state.payload; + return true; + } + else + { + ROS_ERROR("get the servo state: timeout"); + return false; + } + + + } + + bool SagittariusArmReal::arm_get_robot_info(sdk_sagittarius_arm::ArmInfo::Request & req, sdk_sagittarius_arm::ArmInfo::Response & res) + { + // Parse the urdf model to get joint position and velocity limits + if (!ros::param::has("robot_description")) + { + ROS_ERROR("The sagittarius/robot_description parameter was not found!"); + return false; + } + urdf::Model model; + model.initParam(robot_name + "/robot_description"); + for (auto const &joint : all_joints) + { + urdf::JointConstSharedPtr ptr; + if (joint.name == "joint_gripper_left") + { + ptr = model.getJoint("joint_gripper_left"); + res.lower_gripper_limit = ptr->limits->lower; + res.upper_gripper_limit = ptr->limits->upper; + res.use_gripper = true; + } + ptr = model.getJoint(joint.name); + res.lower_joint_limits.push_back(ptr->limits->lower); + res.upper_joint_limits.push_back(ptr->limits->upper); + res.velocity_limits.push_back(ptr->limits->velocity); + res.joint_names.push_back(joint.name); + res.joint_ids.push_back(joint.servo_id); + + } + res.home_pos = home_positions; + res.sleep_pos = sleep_positions; + res.num_joints = joint_num_write; + res.num_single_joints = all_joints.size(); + return true; + } + + void SagittariusArmReal::ControlTorque(const std_msgs::String::ConstPtr &msg) + { + ROS_INFO("ControlTorque:%s.\n",msg->data.c_str()); + if (msg->data == "open") + { + torque_status = true; + pSDKarm->SendArmLockOrFree(1); + } + else + { + torque_status = false; + pSDKarm->SendArmLockOrFree(0); + } + } + + void SagittariusArmReal::JointStatesCb(const sensor_msgs::JointState& cmd_arm) + { + if(pSDKarm!=NULL) + { + angle[0] = cmd_arm.position[0]; + angle[1] = cmd_arm.position[1]; + angle[2] = cmd_arm.position[2]; + angle[3] = cmd_arm.position[3]; + angle[4] = cmd_arm.position[4]; + angle[5] = cmd_arm.position[5]; + if(torque_status) + { + pSDKarm->SendArmAllServerCB(angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); //插补 + arm_set_gripper_linear_position(cmd_arm.position[6]*2); + //pSDKarm->SendArmAllServer(angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); + //ROS_INFO("[%f,%f,%f,%f,%f,%f]", angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); + } + } + + } + + void SagittariusArmReal::arm_joint_trajectory_msg_callback(const trajectory_msgs::JointTrajectory &msg) + { + if (execute_joint_traj == false) + { + std::map joint_order; + uint8_t cntr = 0; + + for (auto const& name:msg.joint_names) + { + joint_order[name] = cntr; + cntr++; + } + jnt_tra_msg.joint_names.clear(); + jnt_tra_msg.points.clear(); + jnt_tra_msg.header = msg.header; + for (auto const& joint:arm_joints) + if (joint.name != "joint_gripper_left") + jnt_tra_msg.joint_names.push_back(joint.name); + + cntr = 0; + size_t pos_size = msg.points.at(0).positions.size(); + size_t vel_size = msg.points.at(0).velocities.size(); + size_t accel_size = msg.points.at(0).accelerations.size(); + while(cntr < msg.points.size()) + { + trajectory_msgs::JointTrajectoryPoint jnt_tra_point_msg; + jnt_tra_point_msg.time_from_start = msg.points.at(cntr).time_from_start; + for (auto const& joint:arm_joints) + { + if (joint.name != "joint_gripper_left") + { + ///ROS_WARN("position =%lf",msg.points.at(cntr).positions.at(joint_order[joint.name])) ; + if (pos_size != 0) + jnt_tra_point_msg.positions.push_back(msg.points.at(cntr).positions.at(joint_order[joint.name])); + if (vel_size != 0) + jnt_tra_point_msg.velocities.push_back(msg.points.at(cntr).velocities.at(joint_order[joint.name])); + if (accel_size != 0) + jnt_tra_point_msg.accelerations.push_back(msg.points.at(cntr).accelerations.at(joint_order[joint.name])); + } + } + jnt_tra_msg.points.push_back(jnt_tra_point_msg); + cntr++; + } + size_t itr = 0; + joint_states = pSDKarm->joint_states; + for (auto const& joint:arm_joints) + { + if (joint.name != "joint_gripper_left") + { + //ROS_WARN("joint.name != gripper,positions.at(%d)=%d",itr, jnt_tra_msg.points[0].positions.at(itr)); + if (!(fabs(jnt_tra_msg.points[0].positions.at(itr) - joint_states.position.at(itr)) < 0.1)) + { + ROS_WARN("%s Servo is not at the correct initial state.", joint_states.name.at(itr).c_str()); + ROS_WARN("Expected state: %f, Actual State: %f.", jnt_tra_msg.points.at(0).positions.at(itr), joint_states.position.at(itr)); + } + itr++; + } + } + ROS_INFO("Succeeded to get joint trajectory!"); + joint_start_time = ros::Time::now().toSec(); + execute_joint_traj = true; + } + else + { + ROS_WARN("Arm joints are still moving"); + } + } + + + void SagittariusArmReal::arm_joint_trajectory_action_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) + { + control_msgs::FollowJointTrajectoryResult result; + if (goal->trajectory.points.size() < 2) + { + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; + joint_action_server->setSucceeded(result); + return; + } + arm_joint_trajectory_msg_callback(goal->trajectory); + ros::Rate r(100); + while (execute_joint_traj) + { + if (joint_action_server->isPreemptRequested()) + { + execute_joint_traj = false; + joint_action_server->setPreempted(); + ROS_INFO("Joint trajectory server preempted by client"); + return; + } + r.sleep(); + } + result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + joint_action_server->setSucceeded(result); + //ROS_WARN("RobotArm::arm_joint_trajectory_action_callback go out"); + } + + void SagittariusArmReal::arm_gripper_trajectory_action_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) + { + control_msgs::FollowJointTrajectoryResult result; + //ROS_WARN("arm_gripper_trajectory_action_callback"); + if (goal->trajectory.points.size() < 2) + { + result.error_code = control_msgs::FollowJointTrajectoryResult::INVALID_GOAL; + gripper_action_server->setSucceeded(result); + return; + } + ROS_WARN("arm_gripper_trajectory_msg_callback"); + arm_gripper_trajectory_msg_callback(goal->trajectory); + ros::Rate r(100); + while (execute_gripper_traj) + { + if (gripper_action_server->isPreemptRequested()) + { + execute_gripper_traj = false; + gripper_action_server->setPreempted(); + ROS_INFO("Gripper trajectory server preempted by client"); + return; + } + r.sleep(); + } + result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL; + //ROS_INFO("gripper_action_server setSucceeded"); + gripper_action_server->setSucceeded(result); + } + + void SagittariusArmReal::arm_gripper_trajectory_msg_callback(const trajectory_msgs::JointTrajectory &msg) + { + if (execute_gripper_traj == false) + { + gripper_tra_msg.joint_names.clear(); + gripper_tra_msg.points.clear(); + gripper_tra_msg = msg; + if (!(fabs(gripper_tra_msg.points.at(0).positions.at(0) - joint_states.position.at(arm_joints.size())) < 0.1)) + { + ROS_WARN("Gripper Servo is not at the correct initial state."); + ROS_WARN("Expected state: %f, Actual State: %f.", gripper_tra_msg.points.at(0).positions.at(0), joint_states.position.at(arm_joints.size())); + } + + ROS_INFO("Succeeded to get gripper trajectory!"); + gripper_start_time = ros::Time::now().toSec(); + execute_gripper_traj = true; + } + else + { + ROS_WARN("Gripper is still moving"); + } + } + /// @brief arm_execute_joint_trajectory 关节的轨迹跟踪定时器 + /// @param - TimerEvent 事件定时器参数 + void SagittariusArmReal::arm_execute_joint_trajectory(const ros::TimerEvent&) + { + static uint8_t cntr = 0; + static int flag = 1; + if (!execute_joint_traj) + { + if (cntr != 0) + { + ROS_INFO("Joint Trajectory stopped."); + cntr = 0; + } + flag = 1; + return; + } + int traj_size = jnt_tra_msg.points.size(); + double time_now = ros::Time::now().toSec() - joint_start_time; + double time_from_start = jnt_tra_msg.points[cntr].time_from_start.toSec(); + //ROS_WARN("now %lf,tfs %lf",time_now, time_from_start); + if(servo_control_trajectory) + { + + if(flag) + { + flag = 0; + uint8_t counter = 0; + double pos_array[joint_num_write]; + //printf("pos_array:"); + for (auto const& pos:jnt_tra_msg.points[traj_size-1].positions) + { + pos_array[counter] = pos; + //printf("=%lf ",pos); + counter++; + } + //printf("\n"); + arm_set_joint_positions(pos_array, 4.0); + + //arm_set_joint_positions(pos_array, (jnt_tra_msg.points[traj_size-1].time_from_start.toSec()-jnt_tra_msg.points[0].time_from_start.toSec())); + } + if (time_now > time_from_start) + { + cntr++; + if (cntr < traj_size) + { + } + else + { + ROS_INFO("Trajectory done being executed."); + execute_joint_traj = false; + cntr = 0; + } + } + } + else + { + + if(time_from_start>0) + time_from_start = time_from_start;//-0.02; + + if (time_now > time_from_start) + { + cntr++; + if (cntr < traj_size) + { + //ROS_WARN("--now %lf,tfs %lf",time_now, time_from_start); + if (1) + { + uint8_t counter = 0; + double pos_array[joint_num_write]; + //printf("pos_array:"); + for (auto const& pos:jnt_tra_msg.points[cntr].positions) + { + pos_array[counter] = pos; + //printf("=%lf ",pos); + counter++; + } + //printf("\n"); + arm_set_joint_positions(pos_array, (jnt_tra_msg.points[cntr].time_from_start.toSec()-jnt_tra_msg.points[cntr-1].time_from_start.toSec())); + //ROS_WARN("now %lf,tfs %lf",time_now, time_from_start); + + } + } + else + { + ROS_INFO("Trajectory done being executed."); + execute_joint_traj = false; + cntr = 0; + } + } + } + } + + /// @brief arm_calculate_gripper_degree_position 距离转换成角度 + /// @param dist - 夹爪的距离值 + short SagittariusArmReal::arm_calculate_gripper_degree_position(const float dist) + { + double half_dist = dist / 2.0; + short result = -(3462*half_dist)*10; + return result; + } + /// @brief arm_set_gripper_linear_position 设置夹爪的位置 + /// @param dist - 夹爪的距离值 + void SagittariusArmReal::arm_set_gripper_linear_position(const float dist) + { + short g_degree = arm_calculate_gripper_degree_position(dist); + //printf("degree is %d\n",g_degree); + arm_set_single_joint_degree_position(g_degree); + } + /// @brief arm_set_single_joint_degree_position 发送单个关节的角度。 + /// @param g_degree - 角度值 + void SagittariusArmReal::arm_set_single_joint_degree_position(short g_degree) + { + if(torque_status) + { + pSDKarm->SendArmEndAction(0,g_degree); + } + } + /// @brief arm_execute_gripper_trajectory 夹爪的轨迹跟踪定时器 + /// @param - TimerEvent 事件定时器参数 + void SagittariusArmReal::arm_execute_gripper_trajectory(const ros::TimerEvent&) + { + static uint8_t cntr = 0; + if (!execute_gripper_traj) + { + if (cntr != 0) + { + ROS_INFO("Gripper Trajectory stopped."); + cntr = 0; + } + return; + } + int traj_size = gripper_tra_msg.points.size(); + double time_now = ros::Time::now().toSec() - gripper_start_time; + double time_from_start = gripper_tra_msg.points.at(cntr).time_from_start.toSec(); + if (time_now > time_from_start) + { + while (time_now > time_from_start && cntr < (traj_size - 1)) + { + cntr++; + time_from_start = gripper_tra_msg.points.at(cntr).time_from_start.toSec(); + } + if (cntr < (traj_size - 1)) + { + arm_set_gripper_linear_position(gripper_tra_msg.points.at(cntr).positions.at(0)*2.0); + } + else + { + arm_set_gripper_linear_position(gripper_tra_msg.points.at(cntr).positions.at(0)*2.0); + ROS_INFO("Trajectory done being executed."); + execute_gripper_traj = false; + cntr = 0; + } + } + } + + /// @brief arm_set_joint_positions - 控制机械臂各个舵机的移动到目标位置 + /// @param joint_positions - 舵机的目标弧度; diff_time - 用时 + void SagittariusArmReal::arm_set_joint_positions(const double joint_positions[], double diff_time) + { + if(pSDKarm!=NULL) + { + angle[0] = joint_positions[0]; + angle[1] = joint_positions[1]; + angle[2] = joint_positions[2]; + angle[3] = joint_positions[3]; + angle[4] = joint_positions[4]; + angle[5] = joint_positions[5]; + short difftime = diff_time*1000; + if(torque_status) + { + //pSDKarm->SendArmAllServerCB(angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); + pSDKarm->SendArmAllServerTime(difftime, angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); + //ROS_INFO("[%f,%f,%f,%f,%f,%f]", angle[0],angle[1],angle[2],angle[3],angle[4],angle[5]); + } + + } + } + /// @brief arm_write_joint_commands - 处理订阅到的机械臂控制命令。并控制机械臂各个舵机的移动到目标位置 + /// @param msg - 自定义消息。机械臂的位置控制。 + void SagittariusArmReal::arm_write_joint_commands(const sdk_sagittarius_arm::ArmRadControl &msg) + { + double joint_positions[msg.rad.size()]; + for (size_t i {0}; i < msg.rad.size(); i++) + joint_positions[i] = msg.rad.at(i); + arm_set_joint_positions(joint_positions, 0.0); + } + /// @brief arm_write_gripper_command - 控制机械臂末端舵机的移动长度 + /// @param msg - + void SagittariusArmReal::arm_write_gripper_command(const std_msgs::Float64 &msg) + { + arm_set_gripper_linear_position(msg.data*2.0); + } + +} + + + + + + + diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ArmInfo.srv b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ArmInfo.srv new file mode 100644 index 0000000..e7e2433 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ArmInfo.srv @@ -0,0 +1,38 @@ +# Get robot information +# +# Note that all data that is returned in a vector is organized such that each index +# corresponds to the joint at the same index as it appears in the joint_states topic. +# Gripper info is not included in the vectors. +# +# Multiple types of robot information are provided, including: +# 1) joint_names - the names of all joints in the robot +# 2) joint_ids - the Dynamixel IDs for all joints in the robot +# 3) lower_joint_limits - the lower joint limits in radians (taken from URDF) +# 4) upper_joint_limits - the upper joint limits in radians (taken from URDF) +# 5) velocity_limits - the velocity limits in rad/s (taken from URDF) +# 6) lower_gripper_limit - the lower gripper limit in radians (taken from URDF) +# 7) upper_gripper_limit - the upper gripper limit in radians (taken from URDF) +# 8) use_gripper - True if the driver node can control the gripper - otherwise, False +# 9) home_pos - home position for each robot; essentially commands all joints +# (excluding gripper) to 0 radians (taken from arm_poses.h) +# 10) sleep_pos - sleep position for each robot; essentially commands all joints +# (excluding gripper) to a specific position in radians so that +# if the driver node is shutdown (which torques off all motors), +# the arm doesn't come crashing down (taken from arm_poses.h) +# 11) num_joints - the number of joints in the arm (excluding gripper) +# 12) num_single_joints - the number of all joints in the robot (includes gripper and any 'single' joints) + +--- +# joint names appear in the same order as they do in the joint_states.name vector +string[] joint_names +int16[] joint_ids +float64[] lower_joint_limits +float64[] upper_joint_limits +float64[] velocity_limits +float64 lower_gripper_limit +float64 upper_gripper_limit +bool use_gripper +float64[] home_pos +float64[] sleep_pos +int8 num_joints +int8 num_single_joints diff --git a/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ServoRtInfo.srv b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ServoRtInfo.srv new file mode 100644 index 0000000..83a32e4 --- /dev/null +++ b/src/spark_driver/arm/sagittarius_arm_ros/sdk_sagittarius_arm/srv/ServoRtInfo.srv @@ -0,0 +1,17 @@ +# Get servo realtime state +# servo_id : 1~7 + +uint8 servo_id + +--- + +# return the realtime state +# speed(50step/s) +# payload(0.1%) +# voltage(0.1V) +# current(1mA) +int16 speed +int16 payload +uint8 voltage +uint16 current +