mirror of
https://github.com/elephantrobotics/mycobot_ros.git
synced 2026-07-03 02:27:04 +00:00
update mycobot 600 gripper
This commit is contained in:
wangWking
commit
69fc6b3694
985 changed files with 89716 additions and 12148 deletions
176
README.md
176
README.md
|
|
@ -20,6 +20,7 @@ Control or simulate myCobot series robots in ROS.
|
|||
* Ubuntu 18.04 / ROS Melodic
|
||||
* Ubuntu 20.04 / ROS Noetic
|
||||
|
||||
* The urdf model of a single gripper in this article is applicable to all machines that support this accessory.
|
||||
<!-- **If your `Atom` is 2.3 or before, or `pymycobot` is 1.\*, Please check branch [before](https://github.com/elephantrobotics/myCobotRos/tree/before)** -->
|
||||
|
||||
## Installation
|
||||
|
|
@ -183,6 +184,14 @@ Please adhere to this project's [code of conduct](CODE_OF_CONDUCT.md).
|
|||
|
||||
|
||||
|
||||
[mycobot 280 JetsonNano Adaptive gripper](./mycobot_description/urdf/mycobot_280_jn/mycobot_280_jn_adaptive_gripper_parallel.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot 280 JetsonNano Parallel gripper](./mycobot_description/urdf/mycobot_280_jn/mycobot_280_jn_parallel_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot 280 Arduino](./mycobot_description/urdf/mycobot_280_arduino/mycobot_280_arduino.urdf)
|
||||
|
||||
|
||||
|
|
@ -215,10 +224,22 @@ Please adhere to this project's [code of conduct](CODE_OF_CONDUCT.md).
|
|||
|
||||
|
||||
|
||||
[mycobot 320 m5 2022 force control gripper](./mycobot_description/urdf/mycobot_320_m5_2022/pro_320_m5_2022_force_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot 320 pi 2022](./mycobot_description/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot 320 pi 2022 gripper](./mycobot_description/urdf/mycobot_320_pi_2022/new_mycobot_pro_320_pi_2022_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot 320 pi 2022 force control gripper](./mycobot_description/urdf/mycobot_320_pi_2022/pro_320_pi_2022_force_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[ultraArm P340](./mycobot_description/urdf/ultraArm_p340/ultraArm_p340.urdf)
|
||||
|
||||
|
||||
|
|
@ -247,6 +268,161 @@ Please adhere to this project's [code of conduct](CODE_OF_CONDUCT.md).
|
|||
|
||||
|
||||
|
||||
## Single Gripper URDF Model Graph
|
||||
|
||||
>> **This urdf model is applicable to all machines that support this accessory**
|
||||
|
||||
[mycobot Adaptive gripper](./mycobot_description/urdf/adaptive_gripper/mycobot_adaptive_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot Parallel gripper](./mycobot_description/urdf/parallel_gripper/mycobot_parallel_gripper.urdf)
|
||||
|
||||
|
||||
|
||||
[mycobot Pro Adaptive gripper](./mycobot_description/urdf/pro_adaptive_gripper/mycobot_pro_adaptive_gripper.urdf) - old version
|
||||
|
||||
|
||||
|
||||
[mycobot Pro Adaptive gripper](./mycobot_description/urdf/pro_adaptive_gripper/new_gripper/mycobot_pro_adaptive_gripper_new.urdf) (new version - Added more details)
|
||||
|
||||
|
||||
|
||||
## MyCobot_280_m5-Gazebo User Guide
|
||||
1. Slider Control
|
||||
The control of the robot arm model's pose in Gazebo through the sliders of joint_state_publisher_gui has now been achieved. Moreover, the pose of the robot arm model in Gazebo and the real robot arm can be controlled simultaneously via the sliders.
|
||||
After confirming that the real robot arm is connected to the computer, check the port to which the robot arm is connected:
|
||||
|
||||
```bash
|
||||
ls /dev/tty*
|
||||
/dev/ttyACM0 or /dev/ttyUSB0
|
||||
```
|
||||
|
||||
The following output results are obtained:
|
||||
|
||||
```bash
|
||||
/dev/tty /dev/tty26 /dev/tty44 /dev/tty62 /dev/ttyS20
|
||||
/dev/tty0 /dev/tty27 /dev/tty45 /dev/tty63 /dev/ttyS21
|
||||
/dev/tty1 /dev/tty28 /dev/tty46 /dev/tty7 /dev/ttyS22
|
||||
/dev/tty10 /dev/tty29 /dev/tty47 /dev/tty8 /dev/ttyS23
|
||||
/dev/tty11 /dev/tty3 /dev/tty48 /dev/tty9 /dev/ttyS24
|
||||
/dev/tty12 /dev/tty30 /dev/tty49 /dev/ttyACM0 (/dev/ttyUSB0)
|
||||
/dev/tty13 /dev/tty31 /dev/tty5 /dev/ttyprintk /dev/ttyS26
|
||||
/dev/tty14 /dev/tty32 /dev/tty50 /dev/ttyS0 /dev/ttyS27
|
||||
/dev/tty15 /dev/tty33 /dev/tty51 /dev/ttyS1 /dev/ttyS28
|
||||
/dev/tty16 /dev/tty34 /dev/tty52 /dev/ttyS10 /dev/ttyS29
|
||||
/dev/tty17 /dev/tty35 /dev/tty53 /dev/ttyS11 /dev/ttyS3
|
||||
/dev/tty18 /dev/tty36 /dev/tty54 /dev/ttyS12 /dev/ttyS30
|
||||
/dev/tty19 /dev/tty37 /dev/tty55 /dev/ttyS13 /dev/ttyS31
|
||||
/dev/tty2 /dev/tty38 /dev/tty56 /dev/ttyS14 /dev/ttyS4
|
||||
/dev/tty20 /dev/tty39 /dev/tty57 /dev/ttyS15 /dev/ttyS5
|
||||
/dev/tty21 /dev/tty4 /dev/tty58 /dev/ttyS16 /dev/ttyS6
|
||||
/dev/tty22 /dev/tty40 /dev/tty59 /dev/ttyS17 /dev/ttyS7
|
||||
/dev/tty23 /dev/tty41 /dev/tty6 /dev/ttyS18 /dev/ttyS8
|
||||
/dev/tty24 /dev/tty42 /dev/tty60 /dev/ttyS19 /dev/ttyS9
|
||||
/dev/tty25 /dev/tty43 /dev/tty61 /dev/ttyS2
|
||||
```
|
||||
|
||||
Open communication and Chmod
|
||||
|
||||
```bash
|
||||
sudo chmod -R 777 /dev/ttyACM0 or sudo chmod -r 777 /dev/ttyUSB0
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/follow_display_gazebo.py
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/slider_control_gazebo.py
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/teleop_keyboard_gazebo.py
|
||||
roscore
|
||||
```
|
||||
|
||||
After confirming the port, open a terminal and enter the following command. Note that you should replace "port" with the value you found in the previous step.
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper slider.launch _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
|
||||
Then open another terminal and enter the following command:
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper slider_control_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
|
||||
Also remember to modify the port number to the one queried in the previous step. If the operation is successful, you will see the following terminal prompt:
|
||||
|
||||
```bash
|
||||
('/dev/ttyACM0', 115200)
|
||||
spin ...
|
||||
```
|
||||
|
||||
|
||||
At this point, you can control the poses of both the mechanical arm model in Gazebo or the real mechanical arm simultaneously by manipulating the sliders in the joint_state_publisher_gui.
|
||||
|
||||
2. Gazebo Model Following
|
||||
The following command can be used to make the model in Gazebo change its pose in accordance with the movement of the actual robotic arm. First, run the launch file:
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper follower.launch _port:=/dev/ttyACM0
|
||||
```
|
||||
|
||||
|
||||
If the program runs successfully, the Gazebo interface will successfully load the robotic arm model, and all joints of the robotic arm model will be in the original pose, that is, [0, 0, 0, 0, 0, 0]. After that, we open the second terminal and run:
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper follow_display_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
|
||||
Now when we control the pose of the actual robotic arm, we can see that the robotic arm in Gazebo will also move to the same pose together.
|
||||
|
||||
3. Keyboard Control
|
||||
We can also use keyboard input to simultaneously control the pose of the robotic arm model in Gazebo and the actual robotic arm. First, open a terminal and enter:
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper teleop_keyboard.launch _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
|
||||
As in the previous part, we will see the robotic arm model loaded into Gazebo, and all joints are at their initial poses. Then we open another terminal and enter:
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper teleop_keyboard_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
|
||||
If the operation is successful, we will see the following output information in the terminal:
|
||||
|
||||
```shell
|
||||
Mycobot_280_m5_gripper Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options (control the angle of each joint):
|
||||
w: joint2_to_joint1++ s: joint2_to_joint1--
|
||||
e: joint3_to_joint2++ d: joint3_to_joint2--
|
||||
r: joint4_to_joint3++ f: joint4_to_joint3--
|
||||
t: joint5_to_joint4++ g: joint5_to_joint4--
|
||||
y: joint6_to_joint5++ h: joint6_to_joint5--
|
||||
u: joint6output_to_joint6++ j: joint6output_to_joint6--
|
||||
o:open gripper p:close gripper
|
||||
Other:
|
||||
1 - Go to home pose
|
||||
q - Quit
|
||||
```
|
||||
|
||||
You can find out which interfaces pymycobot provides in `README.md`.
|
||||
|
||||
|
||||
|
||||
> Note: Version v3.6.0 differentiates interfaces by model. Starting from this version, the MyCobot class will no longer be maintained. For new usage, please refer to the document:
|
||||
|
||||
 
|
||||
|
||||
[MyCobot 280 m5 gazebo中文操作](./mycobot_280/280m5_gazebo_gripper/READMECN.md)
|
||||
|
||||
## Contributors
|
||||
|
||||
Thanks goes to these people ([Emoji Key](https://allcontributors.org/docs/en/emoji-key)):
|
||||
|
|
|
|||
BIN
demo_img/280jn/mycobot_280jn_adaptive_gripper.png
Normal file
BIN
demo_img/280jn/mycobot_280jn_adaptive_gripper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 196 KiB |
BIN
demo_img/280jn/mycobot_280jn_parallel_gripper.png
Normal file
BIN
demo_img/280jn/mycobot_280jn_parallel_gripper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 206 KiB |
BIN
demo_img/320m5_2022/320m5_force_gripper_2022.png
Normal file
BIN
demo_img/320m5_2022/320m5_force_gripper_2022.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 187 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 282 KiB After Width: | Height: | Size: 272 KiB |
Binary file not shown.
|
Before Width: | Height: | Size: 288 KiB After Width: | Height: | Size: 284 KiB |
BIN
demo_img/single_gripper/mycobot_adaptive_gripper.png
Normal file
BIN
demo_img/single_gripper/mycobot_adaptive_gripper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 175 KiB |
BIN
demo_img/single_gripper/mycobot_parallel_gripper.png
Normal file
BIN
demo_img/single_gripper/mycobot_parallel_gripper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 188 KiB |
BIN
demo_img/single_gripper/mycobot_pro_adaptive_gripper.png
Normal file
BIN
demo_img/single_gripper/mycobot_pro_adaptive_gripper.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 210 KiB |
|
|
@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/follow_and_pump.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find mycobot_280)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF，将值合并到 TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics ,mycobot-话题-->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles，坚听并发布真实的角度 -->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,125 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding:utf-8 -*-
|
||||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_communication.srv import (
|
||||
GetCoords,
|
||||
SetCoords,
|
||||
GetAngles,
|
||||
SetAngles,
|
||||
GripperStatus,
|
||||
)
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
def __init__(self):
|
||||
self.br = TransformBroadcaster()
|
||||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have. 如果没有就计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker. 检测 aruco 标记。
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose. 检测标记姿势
|
||||
# argument:
|
||||
# marker corners, 标记角
|
||||
# marker size (meter), 标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec, tvec) = (ret[0], ret[1])
|
||||
(rvec - tvec).any()
|
||||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker. 只需选择第一个检测到的标记
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]
|
||||
# 根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
||||
cv.waitKey(3)
|
||||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
@ -1,197 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# coding:utf-8
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import os
|
||||
|
||||
# Type of message to communicate with mycobot
|
||||
# 与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制mycobot的话题，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# Judging device: ttyUSB* is M5；ttyACM* is wio
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio
|
||||
robot = os.popen("ls /dev/ttyAMA*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
Pin = [2, 5]
|
||||
else:
|
||||
Pin = [20, 21]
|
||||
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# Instantiate the message object. 实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# Deviation value from the real position of mycobot
|
||||
# 与mycobot真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 使用此变量限制，抓取行为仅执行一次
|
||||
flag = False
|
||||
|
||||
# In order to compare whether the QR code moves later
|
||||
# 为了比较二维码后面是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""Post coordinates 发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
coords.rx = rx
|
||||
coords.ry = ry
|
||||
coords.rz = rz
|
||||
coords.speed = 70
|
||||
coords.model = m
|
||||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""Publishing angle. 发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""Publish gripper status. 发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves. 判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
print(o, n)
|
||||
if abs(o - n) < 2:
|
||||
count += 1
|
||||
print(count)
|
||||
if count == 3:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
"""callback function. 回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# Parse out the coordinate value. 解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
pub_pump(True, Pin)
|
||||
# pump on
|
||||
|
||||
pub_coords(x, y, z + 20, -165)
|
||||
time.sleep(1.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
||||
177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# subscribers to mark information, mark信息的订阅者
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding:utf-8 -*-
|
||||
import time
|
||||
|
||||
|
|
@ -8,7 +8,18 @@ from std_msgs.msg import Header
|
|||
from visualization_msgs.msg import Marker
|
||||
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
def talker():
|
||||
|
|
@ -19,7 +30,7 @@ def talker():
|
|||
baud = rospy.get_param("~baud", 115200)
|
||||
print("port: {}, baud: {}\n".format(port, baud))
|
||||
try:
|
||||
mycobot = MyCobot(port, baud)
|
||||
mycobot = MechArm270(port, baud)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
print(
|
||||
|
|
|
|||
|
|
@ -1,65 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# -*- coding:utf-8 -*-
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
|
||||
|
||||
def talker():
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = "/joint1"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
print(type(trans), trans)
|
||||
print(type(rot), rot)
|
||||
|
||||
# marker 标记
|
||||
marker_.header.stamp = now
|
||||
marker_.type = marker_.CUBE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial. 标记初始位置
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
marker_.pose.orientation.x = rot[0]
|
||||
marker_.pose.orientation.y = rot[1]
|
||||
marker_.pose.orientation.z = rot[2]
|
||||
marker_.pose.orientation.w = rot[3]
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -1,4 +1,4 @@
|
|||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python3
|
||||
# -*- coding:utf-8 -*-
|
||||
# license removed for brevity
|
||||
import time
|
||||
|
|
@ -7,7 +7,7 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.srv import GetAngles
|
||||
from mecharm_communication.srv import GetAngles
|
||||
|
||||
|
||||
def talker():
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.msg import MycobotAngles
|
||||
from mecharm_communication.msg import MycobotAngles
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ try:
|
|||
import tkinter as tk
|
||||
except ImportError:
|
||||
import Tkinter as tk
|
||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
from mecharm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import time
|
||||
from rospy import ServiceException
|
||||
|
|
|
|||
|
|
@ -14,7 +14,18 @@ import time
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
|
||||
|
|
@ -40,7 +51,7 @@ def listener():
|
|||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
mc = MechArm270(port, baud)
|
||||
time.sleep(0.05)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding:utf-8 -*-
|
||||
from __future__ import print_function
|
||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
from mecharm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import sys
|
||||
import select
|
||||
|
|
|
|||
|
|
@ -4,9 +4,20 @@ import time
|
|||
import rospy
|
||||
import os
|
||||
import fcntl
|
||||
from mycobot_communication.srv import *
|
||||
from mecharm_communication.srv import *
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
mc = None
|
||||
|
||||
|
|
@ -54,7 +65,7 @@ def create_handle():
|
|||
port = rospy.get_param("~port")
|
||||
baud = rospy.get_param("~baud")
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
mc = MyCobot(port, baud)
|
||||
mc = MechArm270(port, baud)
|
||||
|
||||
|
||||
def create_services():
|
||||
|
|
|
|||
|
|
@ -17,7 +17,18 @@ from mycobot_communication.msg import (
|
|||
MycobotPumpStatus,
|
||||
)
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
class Watcher:
|
||||
|
|
@ -75,7 +86,7 @@ class MycobotTopics(object):
|
|||
port = rospy.get_param("~port", os.popen("ls /dev/ttyACM*").readline()[:-1])
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
self.mc = MyCobot(port, baud)
|
||||
self.mc = MechArm270(port, baud)
|
||||
self.lock = threading.Lock()
|
||||
|
||||
def start(self):
|
||||
|
|
|
|||
|
|
@ -17,10 +17,19 @@ from mycobot_communication.msg import (
|
|||
MycobotPumpStatus,
|
||||
)
|
||||
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
# from pymycobot import MyCobot
|
||||
|
||||
# from pymycobot import MyCobot
|
||||
|
||||
from pymycobot import MyCobotSocket # pi
|
||||
from pymycobot import MyCobotSocket # pi
|
||||
|
||||
|
||||
class Watcher:
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
#!/usr/bin/env python2
|
||||
#!/usr/bin/env python
|
||||
# -*- coding:utf-8 -*-
|
||||
import time
|
||||
import os
|
||||
|
|
@ -8,7 +8,7 @@ import threading
|
|||
|
||||
import rospy
|
||||
|
||||
from mycobot_communication.msg import (
|
||||
from mecharm_communication.msg import (
|
||||
MycobotAngles,
|
||||
MycobotCoords,
|
||||
MycobotSetAngles,
|
||||
|
|
@ -17,7 +17,18 @@ from mycobot_communication.msg import (
|
|||
MycobotPumpStatus,
|
||||
)
|
||||
|
||||
from pymycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
# from pymycobot import MyCobotSocket
|
||||
|
||||
|
|
@ -79,7 +90,7 @@ class MycobotTopics(object):
|
|||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
# self.mc = MyCobotSocket(port, baud) # port
|
||||
# self.mc.connect() #pi
|
||||
self.mc = MyCobot(port, baud)
|
||||
self.mc = MechArm270(port, baud)
|
||||
self.lock = threading.Lock()
|
||||
|
||||
def start(self):
|
||||
|
|
|
|||
|
|
@ -18,7 +18,17 @@ from mycobot_communication.msg import (
|
|||
)
|
||||
|
||||
|
||||
from pymycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.mycobot import MyCobot
|
||||
|
||||
|
||||
class Watcher:
|
||||
|
|
|
|||
|
|
@ -5,8 +5,18 @@ import time
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
# from pymycobot.mypalletizer import MyPalletizer
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
mc = None
|
||||
|
|
@ -30,7 +40,7 @@ def listener():
|
|||
port = rospy.get_param("~port", "/dev/ttyUSB0")
|
||||
baud = rospy.get_param("~baud", 115200)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
mc = MechArm270(port, baud)
|
||||
time.sleep(0.05)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
|
|
|||
|
|
@ -27,9 +27,6 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/follow_and_pump.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find mycobot_280)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,31 +0,0 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF，将值合并到 TF -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics ,mycobot-话题-->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles，坚听并发布真实的角度 -->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,125 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
# -*- coding:utf-8 -*-
|
||||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_communication.srv import (
|
||||
GetCoords,
|
||||
SetCoords,
|
||||
GetAngles,
|
||||
SetAngles,
|
||||
GripperStatus,
|
||||
)
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
def __init__(self):
|
||||
self.br = TransformBroadcaster()
|
||||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img进来
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have. 如果没有就计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker. 检测 aruco 标记。
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose. 检测标记姿势
|
||||
# argument:
|
||||
# marker corners, 标记角
|
||||
# marker size (meter), 标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec, tvec) = (ret[0], ret[1])
|
||||
(rvec - tvec).any()
|
||||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker. 只需选择第一个检测到的标记
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]
|
||||
# 根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
||||
cv.waitKey(3)
|
||||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
@ -1,197 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# coding:utf-8
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import os
|
||||
|
||||
# Type of message to communicate with mycobot
|
||||
# 与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制mycobot的话题，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# Judging device: ttyUSB* is M5；ttyACM* is wio
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio
|
||||
robot = os.popen("ls /dev/ttyAMA*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
Pin = [2, 5]
|
||||
else:
|
||||
Pin = [20, 21]
|
||||
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# Instantiate the message object. 实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# Deviation value from the real position of mycobot
|
||||
# 与mycobot真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 使用此变量限制，抓取行为仅执行一次
|
||||
flag = False
|
||||
|
||||
# In order to compare whether the QR code moves later
|
||||
# 为了比较二维码后面是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""Post coordinates 发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
coords.rx = rx
|
||||
coords.ry = ry
|
||||
coords.rz = rz
|
||||
coords.speed = 70
|
||||
coords.model = m
|
||||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""Publishing angle. 发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""Publish gripper status. 发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves. 判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
print(o, n)
|
||||
if abs(o - n) < 2:
|
||||
count += 1
|
||||
print(count)
|
||||
if count == 3:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
"""callback function. 回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# Parse out the coordinate value. 解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: # Indicates that the target is in a stationary state and can be attempted to grab. 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
pub_pump(True, Pin)
|
||||
# pump on
|
||||
|
||||
pub_coords(x, y, z + 20, -165)
|
||||
time.sleep(1.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
||||
177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# subscribers to mark information, mark信息的订阅者
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -8,7 +8,18 @@ from std_msgs.msg import Header
|
|||
from visualization_msgs.msg import Marker
|
||||
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
def talker():
|
||||
|
|
@ -19,7 +30,7 @@ def talker():
|
|||
baud = rospy.get_param("~baud", 1000000)
|
||||
print("port: {}, baud: {}\n".format(port, baud))
|
||||
try:
|
||||
mycobot = MyCobot(port, baud)
|
||||
mycobot = MechArm270(port, baud)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
print(
|
||||
|
|
|
|||
|
|
@ -1,65 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# -*- coding:utf-8 -*-
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
|
||||
|
||||
def talker():
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = "/joint1"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform("joint1", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
print(type(trans), trans)
|
||||
print(type(rot), rot)
|
||||
|
||||
# marker 标记
|
||||
marker_.header.stamp = now
|
||||
marker_.type = marker_.CUBE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial. 标记初始位置
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
marker_.pose.orientation.x = rot[0]
|
||||
marker_.pose.orientation.y = rot[1]
|
||||
marker_.pose.orientation.z = rot[2]
|
||||
marker_.pose.orientation.w = rot[3]
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -7,7 +7,7 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.srv import GetAngles
|
||||
from mecharm_communication.srv import GetAngles
|
||||
|
||||
|
||||
def talker():
|
||||
|
|
|
|||
|
|
@ -5,7 +5,7 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from mycobot_communication.msg import MycobotAngles
|
||||
from mecharm_communication.msg import MycobotAngles
|
||||
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -4,7 +4,7 @@ try:
|
|||
import tkinter as tk
|
||||
except ImportError:
|
||||
import Tkinter as tk
|
||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
from mecharm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import time
|
||||
from rospy import ServiceException
|
||||
|
|
|
|||
|
|
@ -14,7 +14,18 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
|
||||
|
|
@ -41,7 +52,7 @@ def listener():
|
|||
port = rospy.get_param("~port", "/dev/ttyAMA0") # Select connected device. 选择连接设备
|
||||
baud = rospy.get_param("~baud", 1000000)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
mc = MechArm270(port, baud)
|
||||
time.sleep(0.05)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
|
|
|||
|
|
@ -1,7 +1,7 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding:utf-8 -*-
|
||||
from __future__ import print_function
|
||||
from mycobot_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
from mecharm_communication.srv import GetCoords, SetCoords, GetAngles, SetAngles, GripperStatus
|
||||
import rospy
|
||||
import sys
|
||||
import select
|
||||
|
|
|
|||
|
|
@ -4,7 +4,18 @@ import time
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.mycobot import MyCobot
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MIN_REQUIRE_VERSION = '3.6.1'
|
||||
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) < version.parse(MIN_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be greater than {} or higher. The current version is {}. Please upgrade the library version.'.format(MIN_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot import MechArm270
|
||||
|
||||
|
||||
mc = None
|
||||
|
|
@ -28,7 +39,7 @@ def listener():
|
|||
port = rospy.get_param("~port", "/dev/ttyAMA0")
|
||||
baud = rospy.get_param("~baud", 1000000)
|
||||
print(port, baud)
|
||||
mc = MyCobot(port, baud)
|
||||
mc = MechArm270(port, baud)
|
||||
time.sleep(0.05)
|
||||
mc.set_fresh_mode(1)
|
||||
time.sleep(0.05)
|
||||
|
|
|
|||
|
|
@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/follow_and_pump.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find myarm)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find myarm)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics ，mycobot-话题-->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,123 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_communication.srv import (
|
||||
GetCoords,
|
||||
SetCoords,
|
||||
GetAngles,
|
||||
SetAngles,
|
||||
GripperStatus,
|
||||
)
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
def __init__(self):
|
||||
self.br = TransformBroadcaster()
|
||||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker.检测 aruco 标记
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.处理标记数据
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose. 检测marker位姿。
|
||||
# argument:
|
||||
# marker corners,标记角
|
||||
# marker size (meter),标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec, tvec) = (ret[0], ret[1])
|
||||
(rvec - tvec).any()
|
||||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
||||
cv.waitKey(3)
|
||||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# coding:utf-8
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import os
|
||||
|
||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
Pin = [2, 5]
|
||||
else:
|
||||
Pin = [20, 21]
|
||||
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# instantiate the message object，实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 通过该变量限制，抓取行为只做一次
|
||||
flag = False
|
||||
|
||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""Post coordinates,发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
coords.rx = rx
|
||||
coords.ry = ry
|
||||
coords.rz = rz
|
||||
coords.speed = 70
|
||||
coords.model = m
|
||||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""Publishing angle,发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""Publish gripper status,发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves"""
|
||||
"""判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
print(o, n)
|
||||
if abs(o - n) < 2:
|
||||
count += 1
|
||||
print(count)
|
||||
if count == 3:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
"""callback function,回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# Parse out the coordinate value,解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
pub_pump(True, Pin)
|
||||
# pump on
|
||||
|
||||
pub_coords(x, y, z + 20, -165)
|
||||
time.sleep(1.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
||||
177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# mark 信息的订阅者,subscribers to mark information
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -6,7 +6,17 @@ from sensor_msgs.msg import JointState
|
|||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
|
||||
from pymycobot.myarm import MyArm
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
|
||||
def talker():
|
||||
|
|
|
|||
|
|
@ -1,64 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
|
||||
|
||||
def talker():
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = "/base"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
print(type(trans), trans)
|
||||
print(type(rot), rot)
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = now
|
||||
marker_.type = marker_.CUBE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial,标记位置初始化
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
marker_.pose.orientation.x = rot[0]
|
||||
marker_.pose.orientation.y = rot[1]
|
||||
marker_.pose.orientation.z = rot[2]
|
||||
marker_.pose.orientation.w = rot[3]
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -13,7 +13,17 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.myarm import MyArm
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
|
||||
mc = None
|
||||
|
|
|
|||
|
|
@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/follow_and_pump.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_c650.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find myarm_c650)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="myarm_c650" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyACM0" />
|
||||
<arg name="baud" default="1000000" />
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm_c650/myarm_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find myarm_c650)/config/myarm_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics ，mycobot-话题-->
|
||||
<include file="$(find myarm_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
||||
<node name="real_listener" pkg="myarm_c650" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="myarm_c650" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="myarm_c650" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="myarm_c650" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,123 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_communication.srv import (
|
||||
GetCoords,
|
||||
SetCoords,
|
||||
GetAngles,
|
||||
SetAngles,
|
||||
GripperStatus,
|
||||
)
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
def __init__(self):
|
||||
self.br = TransformBroadcaster()
|
||||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker.检测 aruco 标记
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.处理标记数据
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose. 检测marker位姿。
|
||||
# argument:
|
||||
# marker corners,标记角
|
||||
# marker size (meter),标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec, tvec) = (ret[0], ret[1])
|
||||
(rvec - tvec).any()
|
||||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
||||
cv.waitKey(3)
|
||||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# coding:utf-8
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import os
|
||||
|
||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
Pin = [2, 5]
|
||||
else:
|
||||
Pin = [20, 21]
|
||||
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# instantiate the message object，实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 通过该变量限制，抓取行为只做一次
|
||||
flag = False
|
||||
|
||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""Post coordinates,发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
coords.rx = rx
|
||||
coords.ry = ry
|
||||
coords.rz = rz
|
||||
coords.speed = 70
|
||||
coords.model = m
|
||||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""Publishing angle,发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""Publish gripper status,发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves"""
|
||||
"""判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
print(o, n)
|
||||
if abs(o - n) < 2:
|
||||
count += 1
|
||||
print(count)
|
||||
if count == 3:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
"""callback function,回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# Parse out the coordinate value,解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
pub_pump(True, Pin)
|
||||
# pump on
|
||||
|
||||
pub_coords(x, y, z + 20, -165)
|
||||
time.sleep(1.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
||||
177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# mark 信息的订阅者,subscribers to mark information
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
|
||||
|
||||
def talker():
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = "/base"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
print(type(trans), trans)
|
||||
print(type(rot), rot)
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = now
|
||||
marker_.type = marker_.CUBE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial,标记位置初始化
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
marker_.pose.orientation.x = rot[0]
|
||||
marker_.pose.orientation.y = rot[1]
|
||||
marker_.pose.orientation.z = rot[2]
|
||||
marker_.pose.orientation.w = rot[3]
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -5,9 +5,17 @@ import rospy
|
|||
import os
|
||||
import fcntl
|
||||
from myarm_communication.srv import *
|
||||
|
||||
from pymycobot.myarm import MyArm
|
||||
from pymycobot.myarmc import MyArmC
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
mc = None
|
||||
|
||||
|
|
@ -57,7 +65,7 @@ def create_handle():
|
|||
port = rospy.get_param("~port")
|
||||
baud = rospy.get_param("~baud")
|
||||
rospy.loginfo("%s,%s" % (port, baud))
|
||||
mc = MyArmC(port, baud)
|
||||
mc = MyArm(port, baud)
|
||||
|
||||
|
||||
def create_services():
|
||||
|
|
|
|||
|
|
@ -17,7 +17,17 @@ from myarm_communication.msg import (
|
|||
MyArmPumpStatus,
|
||||
)
|
||||
|
||||
from pymycobot.myarm import MyArm
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
|
||||
class Watcher:
|
||||
|
|
|
|||
|
|
@ -17,9 +17,17 @@ from myarm_communication.msg import (
|
|||
MyArmPumpStatus,
|
||||
)
|
||||
|
||||
|
||||
from pymycobot import MyArm
|
||||
# from pymyarm import myarmSocket
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
|
||||
class Watcher:
|
||||
|
|
|
|||
|
|
@ -26,9 +26,6 @@ catkin_install_python(PROGRAMS
|
|||
scripts/teleop_keyboard.py
|
||||
scripts/listen_real.py
|
||||
scripts/listen_real_of_topic.py
|
||||
scripts/detect_marker.py
|
||||
scripts/following_marker.py
|
||||
scripts/follow_and_pump.py
|
||||
scripts/simple_gui.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
|
||||
)
|
||||
|
|
|
|||
|
|
@ -1,18 +0,0 @@
|
|||
<launch>
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/myarm_300_pi/myarm_300_pi.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find myarm)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="true" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<include file="$(find myarm)/launch/slider_control.launch">
|
||||
<arg name="model" value="$(arg model)" />
|
||||
<arg name="rvizconfig" value="$(arg rvizconfig)" />
|
||||
<arg name="gui" value="$(arg gui)" />
|
||||
</include>
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -1,30 +0,0 @@
|
|||
<launch>
|
||||
<!-- Select connecting device and serial port ，选择连接设备及串口-->
|
||||
<arg name="port" default="/dev/ttyAMA0" />
|
||||
<arg name="baud" default="115200" />
|
||||
<!-- Load file model ，加载文件模型-->
|
||||
<arg name="model" default="$(find mycobot_description)/urdf/mycobot/mycobot_urdf.urdf"/>
|
||||
<arg name="rvizconfig" default="$(find mycobot_280)/config/mycobot_with_marker.rviz" />
|
||||
<arg name="gui" default="false" />
|
||||
|
||||
<arg name="num" default="0" />
|
||||
|
||||
<param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />
|
||||
|
||||
<!-- Combinejoin values to TF ，将值合并到TF-->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
|
||||
<!-- Show in Rviz -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
|
||||
|
||||
<!-- mycobot-topics ，mycobot-话题-->
|
||||
<include file="$(find mycobot_communication)/launch/communication_topic.launch">
|
||||
<arg name="port" value="$(arg port)" />
|
||||
<arg name="baud" value="$(arg baud)" />
|
||||
</include>
|
||||
<!-- listen and pub the real angles ，监听并发布真实的角度-->
|
||||
<node name="real_listener" pkg="mycobot_280" type="listen_real_of_topic.py" />
|
||||
<!-- vision node -->
|
||||
<node name="opencv_camera" pkg="mycobot_280" type="opencv_camera" args="$(arg num)"/>
|
||||
<node name="detect_marker" pkg="mycobot_280" type="detect_marker.py" />
|
||||
<node name="following_marker" pkg="mycobot_280" type="following_marker.py" />
|
||||
</launch>
|
||||
|
|
@ -3,7 +3,7 @@
|
|||
name="model" />
|
||||
<param
|
||||
name="robot_description"
|
||||
textfile="$(find mycobot_description)/urdf/myarmM/myarmM.urdf" />
|
||||
textfile="$(find mycobot_description)/urdf/myarm_m/myarm_m.urdf.xacro" />
|
||||
<node
|
||||
name="joint_state_publisher_gui"
|
||||
pkg="joint_state_publisher_gui"
|
||||
|
|
@ -19,4 +19,4 @@
|
|||
args="-d $(find tw_robot)/urdf.rviz" /> -->
|
||||
<node name="rviz" pkg="rviz" type="rviz" />
|
||||
|
||||
</launch>
|
||||
</launch>
|
||||
|
|
|
|||
|
|
@ -1,123 +0,0 @@
|
|||
#!/usr/bin/env python
|
||||
import rospy
|
||||
import cv2 as cv
|
||||
import numpy as np
|
||||
from cv_bridge import CvBridge, CvBridgeError
|
||||
from sensor_msgs.msg import Image
|
||||
import tf
|
||||
from tf.broadcaster import TransformBroadcaster
|
||||
import tf_conversions
|
||||
from mycobot_communication.srv import (
|
||||
GetCoords,
|
||||
SetCoords,
|
||||
GetAngles,
|
||||
SetAngles,
|
||||
GripperStatus,
|
||||
)
|
||||
|
||||
|
||||
class ImageConverter:
|
||||
def __init__(self):
|
||||
self.br = TransformBroadcaster()
|
||||
self.bridge = CvBridge()
|
||||
self.aruco_dict = cv.aruco.Dictionary_get(cv.aruco.DICT_6X6_250)
|
||||
self.aruo_params = cv.aruco.DetectorParameters_create()
|
||||
calibrationParams = cv.FileStorage(
|
||||
"calibrationFileName.xml", cv.FILE_STORAGE_READ
|
||||
)
|
||||
self.dist_coeffs = calibrationParams.getNode("distCoeffs").mat()
|
||||
self.camera_matrix = None
|
||||
# subscriber, listen wether has img come in. 订阅者，监听是否有img
|
||||
self.image_sub = rospy.Subscriber("/camera/image", Image, self.callback)
|
||||
|
||||
def callback(self, data):
|
||||
"""Callback function.
|
||||
|
||||
Process image with OpenCV, detect Mark to get the pose. Then acccording the
|
||||
pose to transforming.
|
||||
"""
|
||||
try:
|
||||
# trans `rgb` to `gbr` for opencv. 将 `rgb` 转换为 opencv 的 `gbr`。
|
||||
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
size = cv_image.shape
|
||||
focal_length = size[1]
|
||||
center = [size[1] / 2, size[0] / 2]
|
||||
if self.camera_matrix is None:
|
||||
# calc the camera matrix, if don't have.如果没有，则计算相机矩阵
|
||||
self.camera_matrix = np.array(
|
||||
[
|
||||
[focal_length, 0, center[0]],
|
||||
[0, focal_length, center[1]],
|
||||
[0, 0, 1],
|
||||
],
|
||||
dtype=np.float32,
|
||||
)
|
||||
gray = cv.cvtColor(cv_image, cv.COLOR_BGR2GRAY)
|
||||
# detect aruco marker.检测 aruco 标记
|
||||
ret = cv.aruco.detectMarkers(gray, self.aruco_dict, parameters=self.aruo_params)
|
||||
corners, ids = ret[0], ret[1]
|
||||
# process marker data.处理标记数据
|
||||
if len(corners) > 0:
|
||||
if ids is not None:
|
||||
# print('corners:', corners, 'ids:', ids)
|
||||
|
||||
# detect marker pose. 检测marker位姿。
|
||||
# argument:
|
||||
# marker corners,标记角
|
||||
# marker size (meter),标记尺寸（米）
|
||||
ret = cv.aruco.estimatePoseSingleMarkers(
|
||||
corners, 0.05, self.camera_matrix, self.dist_coeffs
|
||||
)
|
||||
(rvec, tvec) = (ret[0], ret[1])
|
||||
(rvec - tvec).any()
|
||||
|
||||
print("rvec:", rvec, "tvec:", tvec)
|
||||
|
||||
# just select first one detected marker.只需选择第一个检测到的标记。
|
||||
for i in range(rvec.shape[0]):
|
||||
cv.aruco.drawDetectedMarkers(cv_image, corners)
|
||||
cv.aruco.drawAxis(
|
||||
cv_image,
|
||||
self.camera_matrix,
|
||||
self.dist_coeffs,
|
||||
rvec[i, :, :],
|
||||
tvec[i, :, :],
|
||||
0.03,
|
||||
)
|
||||
|
||||
xyz = tvec[0, 0, :]
|
||||
xyz = [xyz[0] - 0.045, xyz[1], xyz[2] - 0.03]
|
||||
|
||||
# get quaternion for ros. 为ros获取四元数
|
||||
euler = rvec[0, 0, :]
|
||||
tf_change = tf.transformations.quaternion_from_euler(
|
||||
euler[0], euler[1], euler[2]
|
||||
)
|
||||
print("tf_change:", tf_change)
|
||||
|
||||
# trans pose according [joint1]，根据 [joint1] 变换姿势
|
||||
self.br.sendTransform(
|
||||
xyz, tf_change, rospy.Time.now(), "basic_shapes", "joint6_flange"
|
||||
)
|
||||
|
||||
# [x, y, z, -172, 3, -46.8]
|
||||
cv.imshow("Image", cv_image)
|
||||
|
||||
cv.waitKey(3)
|
||||
try:
|
||||
pass
|
||||
except CvBridgeError as e:
|
||||
print(e)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
rospy.init_node("detect_marker")
|
||||
rospy.loginfo("Starting cv_bridge_test node")
|
||||
ImageConverter()
|
||||
rospy.spin()
|
||||
except KeyboardInterrupt:
|
||||
print("Shutting down cv_bridge_test node.")
|
||||
cv.destroyAllWindows()
|
||||
|
|
@ -1,194 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
# coding:utf-8
|
||||
import rospy
|
||||
from visualization_msgs.msg import Marker
|
||||
import time
|
||||
import os
|
||||
|
||||
# Type of message communicated with mycobot，与 mycobot 通信的消息类型
|
||||
from mycobot_communication.msg import MycobotSetAngles, MycobotSetCoords, MycobotPumpStatus
|
||||
|
||||
|
||||
rospy.init_node("gipper_subscriber", anonymous=True)
|
||||
|
||||
# Control the topic of mycobot, followed by angle, coordinates, gripper
|
||||
# 控制 mycobot 的 topic，依次是角度、坐标、夹爪
|
||||
angle_pub = rospy.Publisher("mycobot/angles_goal",
|
||||
MycobotSetAngles, queue_size=5)
|
||||
coord_pub = rospy.Publisher("mycobot/coords_goal",
|
||||
MycobotSetCoords, queue_size=5)
|
||||
# 判断设备：ttyUSB*为M5；ttyACM*为wio，Judging equipment: ttyUSB* is M5；ttyACM* is wio
|
||||
robot = os.popen("ls /dev/ttyUSB*").readline()
|
||||
|
||||
if "dev" in robot:
|
||||
Pin = [2, 5]
|
||||
else:
|
||||
Pin = [20, 21]
|
||||
|
||||
pump_pub = rospy.Publisher("mycobot/pump_status",
|
||||
MycobotPumpStatus, queue_size=5)
|
||||
|
||||
# instantiate the message object，实例化消息对象
|
||||
angles = MycobotSetAngles()
|
||||
coords = MycobotSetCoords()
|
||||
pump = MycobotPumpStatus()
|
||||
|
||||
# Deviation value from mycobot's real position,与 mycobot 真实位置的偏差值
|
||||
x_offset = -20
|
||||
y_offset = 20
|
||||
z_offset = 110
|
||||
|
||||
# With this variable limit, the fetching behavior is only done once
|
||||
# 通过该变量限制，抓取行为只做一次
|
||||
flag = False
|
||||
|
||||
# In order to compare whether the QR code moves later,为了后面比较二维码是否移动
|
||||
temp_x = temp_y = temp_z = 0.0
|
||||
|
||||
temp_time = time.time()
|
||||
|
||||
|
||||
def pub_coords(x, y, z, rx=-150, ry=10, rz=-90, sp=70, m=2):
|
||||
"""Post coordinates,发布坐标"""
|
||||
coords.x = x
|
||||
coords.y = y
|
||||
coords.z = z
|
||||
coords.rx = rx
|
||||
coords.ry = ry
|
||||
coords.rz = rz
|
||||
coords.speed = 70
|
||||
coords.model = m
|
||||
# print(coords)
|
||||
coord_pub.publish(coords)
|
||||
|
||||
|
||||
def pub_angles(a, b, c, d, e, f, sp):
|
||||
"""Publishing angle,发布角度"""
|
||||
angles.joint_1 = float(a)
|
||||
angles.joint_2 = float(b)
|
||||
angles.joint_3 = float(c)
|
||||
angles.joint_4 = float(d)
|
||||
angles.joint_5 = float(e)
|
||||
angles.joint_6 = float(f)
|
||||
angles.speed = sp
|
||||
angle_pub.publish(angles)
|
||||
|
||||
|
||||
def pub_pump(flag, Pin):
|
||||
"""Publish gripper status,发布夹爪状态"""
|
||||
pump.Status = flag
|
||||
pump.Pin1 = Pin[0]
|
||||
pump.Pin2 = Pin[1]
|
||||
pump_pub.publish(pump)
|
||||
|
||||
|
||||
def target_is_moving(x, y, z):
|
||||
"""Determine whether the target moves"""
|
||||
"""判断目标是否移动"""
|
||||
count = 0
|
||||
for o, n in zip((x, y, z), (temp_x, temp_y, temp_z)):
|
||||
print(o, n)
|
||||
if abs(o - n) < 2:
|
||||
count += 1
|
||||
print(count)
|
||||
if count == 3:
|
||||
return False
|
||||
return True
|
||||
|
||||
|
||||
def grippercallback(data):
|
||||
"""callback function,回调函数"""
|
||||
global flag, temp_x, temp_y, temp_z
|
||||
# rospy.loginfo('gripper_subscriber get date :%s', data)
|
||||
if flag:
|
||||
return
|
||||
|
||||
# Parse out the coordinate value,解析出坐标值
|
||||
# pump length: 88mm
|
||||
x = float(format(data.pose.position.x * 1000, ".2f"))
|
||||
y = float(format(data.pose.position.y * 1000, ".2f"))
|
||||
z = float(format(data.pose.position.z * 1000, ".2f"))
|
||||
|
||||
# When the running time is less than 30s, or the target position is still changing, perform tracking behavior
|
||||
# 当运行时间小于 30s，或目标位置还在改变时，进行追踪行为
|
||||
if (
|
||||
time.time() - temp_time < 30
|
||||
or (temp_x == temp_y == temp_z == 0.0)
|
||||
or target_is_moving(x - x_offset, y - y_offset, z)
|
||||
):
|
||||
|
||||
x -= x_offset
|
||||
y -= y_offset
|
||||
pub_coords(x - 20, y, 280)
|
||||
time.sleep(0.1)
|
||||
|
||||
temp_x, temp_y, temp_z = x, y, z
|
||||
return
|
||||
else: #Indicates that the target is stationary and can try to grab, 表示目标处于静止状态，可以尝试抓取
|
||||
|
||||
print(x, y, z)
|
||||
|
||||
# detect heigth + pump height + limit height + offset
|
||||
x += x_offset
|
||||
y += y_offset
|
||||
z = z + 88 + z_offset
|
||||
|
||||
pub_coords(x, y, z)
|
||||
time.sleep(2.5)
|
||||
|
||||
# down
|
||||
for i in range(1, 17):
|
||||
pub_coords(x, y, z - i * 5, rx=-160, sp=10)
|
||||
time.sleep(0.1)
|
||||
|
||||
time.sleep(2)
|
||||
|
||||
pub_pump(True, Pin)
|
||||
# pump on
|
||||
|
||||
pub_coords(x, y, z + 20, -165)
|
||||
time.sleep(1.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
put_z = 140
|
||||
pub_coords(39.4, -174.7, put_z, -177.13, -4.13, -152.59, 70, 2)
|
||||
time.sleep(1.5)
|
||||
|
||||
for i in range(1, 8):
|
||||
pub_coords(39.4, -174.7, put_z - i * 5, -
|
||||
177.13, -4.13, -152.59, 15, 2)
|
||||
time.sleep(0.1)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
time.sleep(1.5)
|
||||
|
||||
# finally
|
||||
flag = True
|
||||
|
||||
|
||||
def main():
|
||||
for _ in range(10):
|
||||
# pub_coords(150, 20, 220, -175, 0, -90, 70, 2)
|
||||
pub_angles(0, 30, -50, -40, 0, 0, 50)
|
||||
# pub_angles(random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), random.randint(-30, 30), 70)
|
||||
time.sleep(0.5)
|
||||
|
||||
pub_pump(False, Pin)
|
||||
# time.sleep(2.5)
|
||||
|
||||
# mark 信息的订阅者,subscribers to mark information
|
||||
rospy.Subscriber("visualization_marker", Marker,
|
||||
grippercallback, queue_size=1)
|
||||
|
||||
print("gripper test")
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
#!/usr/bin/env python2
|
||||
import time
|
||||
|
||||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
from std_msgs.msg import Header
|
||||
from visualization_msgs.msg import Marker
|
||||
import tf
|
||||
|
||||
|
||||
def talker():
|
||||
rospy.init_node("following_marker", anonymous=True)
|
||||
|
||||
pub_marker = rospy.Publisher("visualization_marker", Marker, queue_size=10)
|
||||
rate = rospy.Rate(20)
|
||||
|
||||
listener = tf.TransformListener()
|
||||
|
||||
marker_ = Marker()
|
||||
marker_.header.frame_id = "/base"
|
||||
marker_.ns = "basic_cube"
|
||||
|
||||
print("publishing ...")
|
||||
while not rospy.is_shutdown():
|
||||
now = rospy.Time.now() - rospy.Duration(0.1)
|
||||
|
||||
try:
|
||||
trans, rot = listener.lookupTransform("base", "basic_shapes", now)
|
||||
except Exception as e:
|
||||
print(e)
|
||||
continue
|
||||
|
||||
print(type(trans), trans)
|
||||
print(type(rot), rot)
|
||||
|
||||
# marker
|
||||
marker_.header.stamp = now
|
||||
marker_.type = marker_.CUBE
|
||||
marker_.action = marker_.ADD
|
||||
marker_.scale.x = 0.04
|
||||
marker_.scale.y = 0.04
|
||||
marker_.scale.z = 0.04
|
||||
|
||||
# marker position initial,标记位置初始化
|
||||
marker_.pose.position.x = trans[0]
|
||||
marker_.pose.position.y = trans[1]
|
||||
marker_.pose.position.z = trans[2]
|
||||
marker_.pose.orientation.x = rot[0]
|
||||
marker_.pose.orientation.y = rot[1]
|
||||
marker_.pose.orientation.z = rot[2]
|
||||
marker_.pose.orientation.w = rot[3]
|
||||
|
||||
marker_.color.a = 1.0
|
||||
marker_.color.g = 1.0
|
||||
pub_marker.publish(marker_)
|
||||
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
talker()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -1,163 +0,0 @@
|
|||
#!/usr/bin/env python3
|
||||
# -*- coding: utf-8 -*-
|
||||
|
||||
import sys
|
||||
import rospy
|
||||
import moveit_commander
|
||||
import moveit_msgs.msg
|
||||
import geometry_msgs.msg
|
||||
import tf
|
||||
|
||||
|
||||
class MoveItPlanningDemo:
|
||||
def __init__(self):
|
||||
rospy.init_node('moveit_avoid_obstacles', anonymous=True)
|
||||
# 初始化MoveIt
|
||||
moveit_commander.roscpp_initialize(sys.argv)
|
||||
|
||||
# 创建RobotCommander对象
|
||||
self.robot = moveit_commander.RobotCommander()
|
||||
|
||||
# 创建PlanningSceneInterface对象
|
||||
self.scene = moveit_commander.PlanningSceneInterface()
|
||||
|
||||
# 创建MoveGroupCommander对象
|
||||
self.arm_group = moveit_commander.MoveGroupCommander("arm_group")
|
||||
|
||||
# 获取末端关节的名称
|
||||
self.end_effector_link = self.arm_group.get_end_effector_link()
|
||||
|
||||
# 设置目标位置所使用的坐标参考系
|
||||
self.reference_frame = 'base'
|
||||
self.arm_group.set_pose_reference_frame(self.reference_frame)
|
||||
|
||||
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
|
||||
self.arm_group.set_goal_position_tolerance(0.01)
|
||||
self.arm_group.set_goal_orientation_tolerance(0.05)
|
||||
|
||||
# 当运动规划失败后，允许重新规划
|
||||
self.arm_group.allow_replanning(True)
|
||||
# 设置规划的最大时间为20秒
|
||||
self.arm_group.set_planning_time(20)
|
||||
# 设置规划尝试次数为10次（或者更大的值）
|
||||
self.arm_group.set_num_planning_attempts(20)
|
||||
|
||||
|
||||
def add_scene(self):
|
||||
# 添加第一个圆柱作为障碍物（垂直于平面）
|
||||
cylinder1_pose = geometry_msgs.msg.PoseStamped()
|
||||
cylinder1_pose.header.frame_id = self.robot.get_planning_frame()
|
||||
cylinder1_pose.pose.position.x = 0.15
|
||||
cylinder1_pose.pose.position.y = 0
|
||||
cylinder1_pose.pose.position.z = 0.30
|
||||
cylinder1_pose.pose.orientation.w = 1.0
|
||||
self.scene.add_cylinder("cylinder1", cylinder1_pose, height=0.6, radius=0.01)
|
||||
|
||||
# 添加第二个圆柱作为障碍物（水平于平面，构成十字架）
|
||||
cylinder2_pose = geometry_msgs.msg.PoseStamped()
|
||||
cylinder2_pose.header.frame_id = self.robot.get_planning_frame()
|
||||
cylinder2_pose.pose.position.x = 0.15
|
||||
cylinder2_pose.pose.position.y = 0
|
||||
cylinder2_pose.pose.position.z = 0.40
|
||||
cylinder2_pose.pose.orientation.w = 1.0
|
||||
cylinder2_pose.pose.orientation.x = 0.707 # 围绕x轴旋转90度（水平方向）
|
||||
cylinder2_pose.pose.orientation.y = 0.0
|
||||
cylinder2_pose.pose.orientation.z = 0.0
|
||||
cylinder2_pose.pose.orientation.w = 0.707
|
||||
self.scene.add_cylinder("cylinder2", cylinder2_pose, height=0.6, radius=0.02)
|
||||
# 发布当前场景信息
|
||||
planning_scene = moveit_msgs.msg.PlanningScene()
|
||||
planning_scene.world.collision_objects.extend(self.scene.get_objects().values())
|
||||
planning_scene.is_diff = True
|
||||
|
||||
planning_scene_publisher = rospy.Publisher('/planning_scene', moveit_msgs.msg.PlanningScene, queue_size=1)
|
||||
planning_scene_publisher.publish(planning_scene)
|
||||
rospy.sleep(2)
|
||||
|
||||
def robot_move(self):
|
||||
|
||||
# 控制机械臂回到初始化位置
|
||||
self.arm_group.set_named_target('init_pose')
|
||||
self.arm_group.go()
|
||||
rospy.sleep(3)
|
||||
|
||||
|
||||
# 设置机械臂运动的目标点，使用笛卡尔空间坐标位置表示（单位：米），姿态使用四元数表示
|
||||
target_pose = geometry_msgs.msg.PoseStamped()
|
||||
target_pose.header.frame_id = self.reference_frame
|
||||
target_pose.header.stamp = rospy.Time.now()
|
||||
target_pose.pose.position.x = 0.142 # 设置目标点的x坐标
|
||||
target_pose.pose.position.y = -0.140 # 设置目标点的y坐标
|
||||
target_pose.pose.position.z = 0.075 # 设置目标点的z坐标
|
||||
target_pose.pose.orientation.x = 0.026
|
||||
target_pose.pose.orientation.y = 1.0
|
||||
target_pose.pose.orientation.z = 0.0
|
||||
target_pose.pose.orientation.w = 0.014
|
||||
|
||||
# 更新当前的位姿
|
||||
self.arm_group.set_start_state_to_current_state()
|
||||
|
||||
# 获取机械臂当前的关节状态
|
||||
current_joint_values = self.arm_group.get_current_joint_values()
|
||||
|
||||
# 打印当前关节状态
|
||||
print("Current Joint Values:", current_joint_values)
|
||||
print("end Joint Values:", self.end_effector_link)
|
||||
# 设置机械臂的目标姿态
|
||||
self.arm_group.set_pose_target(target_pose, self.end_effector_link)
|
||||
# 进行运动规划
|
||||
plan = self.arm_group.plan()
|
||||
# print('plan point:', plan[1])
|
||||
# 执行运动
|
||||
self.arm_group.execute(plan[1])
|
||||
rospy.sleep(3)
|
||||
# 获取末端执行器的姿态
|
||||
end_effector_pose = self.arm_group.get_current_pose().pose
|
||||
|
||||
# 打印末端执行器的坐标位置
|
||||
print("End Effector Position:", end_effector_pose.position)
|
||||
print("End Effector Orientation:", end_effector_pose.orientation)
|
||||
# 控制机械臂末端向右移动5cm 參數1是代表y， 0,1,2,3,4,5 代表xyzrpy
|
||||
# self.arm_group.shift_pose_target(1, 0.22, self.end_effector_link)
|
||||
# self.arm_group.go()
|
||||
# rospy.sleep(5)
|
||||
|
||||
# 设置机械臂的目标位置，使用7轴的位置数据进行描述（单位：弧度）
|
||||
# joint_pose = [0.2967, 0, 0, -1.57000, 0, -1.3439, 0]
|
||||
# joint_pose = [0.2967, 0, 0, 0, 0, -1.3439, 0]
|
||||
# arm_group.set_joint_value_target(joint_pose)
|
||||
|
||||
# 控制机械臂完成运动
|
||||
# arm_group.go()
|
||||
# rospy.sleep(10)
|
||||
# 控制机械臂回到初始化位置
|
||||
# arm_group.set_named_target('init_pose')
|
||||
# arm_group.go()
|
||||
|
||||
def run(self):
|
||||
# 移除所有障碍物
|
||||
# self.scene.remove_world_object("cylinder1")
|
||||
# self.scene.remove_world_object("cylinder2")
|
||||
# 没有障碍物运行一次
|
||||
# self.robot_move()
|
||||
|
||||
# 增加障碍物
|
||||
self.add_scene()
|
||||
rospy.sleep(3)
|
||||
# 获取当前场景中的所有障碍物
|
||||
current_obstacles = self.scene.get_known_object_names()
|
||||
rospy.loginfo("Current obstacles in the scene: %s", current_obstacles)
|
||||
rospy.sleep(2)
|
||||
# 有障碍物后再运行一次
|
||||
self.robot_move()
|
||||
# 关闭MoveIt
|
||||
moveit_commander.roscpp_shutdown()
|
||||
moveit_commander.os._exit(0)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
obstacle = MoveItPlanningDemo()
|
||||
obstacle.run()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
|
@ -14,7 +14,17 @@ import math
|
|||
import rospy
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from pymycobot.myarm import MyArm
|
||||
import pymycobot
|
||||
from packaging import version
|
||||
# min low version require
|
||||
MAX_REQUIRE_VERSION = '3.5.3'
|
||||
current_verison = pymycobot.__version__
|
||||
print('current pymycobot library version: {}'.format(current_verison))
|
||||
if version.parse(current_verison) > version.parse(MAX_REQUIRE_VERSION):
|
||||
raise RuntimeError('The version of pymycobot library must be less than {} . The current version is {}. Please downgrade the library version.'.format(MAX_REQUIRE_VERSION, current_verison))
|
||||
else:
|
||||
print('pymycobot library version meets the requirements!')
|
||||
from pymycobot.myarm import MyArm
|
||||
|
||||
|
||||
mc = None
|
||||
|
|
|
|||
10
mycobot_280/280m5_gazebo_gripper/CMakeLists.txt
Normal file
10
mycobot_280/280m5_gazebo_gripper/CMakeLists.txt
Normal file
|
|
@ -0,0 +1,10 @@
|
|||
cmake_minimum_required(VERSION 3.1.3)
|
||||
project(280m5_gazebo_gripper)
|
||||
|
||||
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})
|
||||
140
mycobot_280/280m5_gazebo_gripper/READMECN.md
Normal file
140
mycobot_280/280m5_gazebo_gripper/READMECN.md
Normal file
|
|
@ -0,0 +1,140 @@
|
|||
### 本地 1.操作流程
|
||||
#### 1.1 安装前提
|
||||
|
||||
要使用此包，需先安装[Python api](https://github.com/elephantrobotics/pymycobot.git)库。
|
||||
|
||||
```bash
|
||||
pip install pymycobot --user
|
||||
ros1 noetic
|
||||
```
|
||||
|
||||
#### 1.2 包的下载与安装
|
||||
|
||||
下载包到你的ros工作空间中
|
||||
|
||||
```bash
|
||||
$ cd ~/catkin_ws/src
|
||||
$ git clone https://github.com/jiaweilong66/280m5_gripper_gazebo.git
|
||||
$ cd ~/catkin_ws
|
||||
$ catkin_make
|
||||
$ source devel/setup.bash
|
||||
```
|
||||
MyCobot_280_m5-Gazebo使用说明
|
||||
1. 滑块控制
|
||||
现已实现通过joint_state_publisher_gui的滑块控制机械臂模型在Gazebo中的位姿
|
||||
确认将真实的机械臂连接到电脑以后，查看机械臂连接的端口：
|
||||
|
||||
```bash
|
||||
ls /dev/tty*
|
||||
/dev/ttyACM0 or /dev/ttyUSB0
|
||||
```
|
||||
|
||||
得到如下的输出结果：
|
||||
|
||||
```bash
|
||||
/dev/tty /dev/tty26 /dev/tty44 /dev/tty62 /dev/ttyS20
|
||||
/dev/tty0 /dev/tty27 /dev/tty45 /dev/tty63 /dev/ttyS21
|
||||
/dev/tty1 /dev/tty28 /dev/tty46 /dev/tty7 /dev/ttyS22
|
||||
/dev/tty10 /dev/tty29 /dev/tty47 /dev/tty8 /dev/ttyS23
|
||||
/dev/tty11 /dev/tty3 /dev/tty48 /dev/tty9 /dev/ttyS24
|
||||
/dev/tty12 /dev/tty30 /dev/tty49 /dev/ttyACM0 (/dev/ttyUSB0)
|
||||
/dev/tty13 /dev/tty31 /dev/tty5 /dev/ttyprintk /dev/ttyS26
|
||||
/dev/tty14 /dev/tty32 /dev/tty50 /dev/ttyS0 /dev/ttyS27
|
||||
/dev/tty15 /dev/tty33 /dev/tty51 /dev/ttyS1 /dev/ttyS28
|
||||
/dev/tty16 /dev/tty34 /dev/tty52 /dev/ttyS10 /dev/ttyS29
|
||||
/dev/tty17 /dev/tty35 /dev/tty53 /dev/ttyS11 /dev/ttyS3
|
||||
/dev/tty18 /dev/tty36 /dev/tty54 /dev/ttyS12 /dev/ttyS30
|
||||
/dev/tty19 /dev/tty37 /dev/tty55 /dev/ttyS13 /dev/ttyS31
|
||||
/dev/tty2 /dev/tty38 /dev/tty56 /dev/ttyS14 /dev/ttyS4
|
||||
/dev/tty20 /dev/tty39 /dev/tty57 /dev/ttyS15 /dev/ttyS5
|
||||
/dev/tty21 /dev/tty4 /dev/tty58 /dev/ttyS16 /dev/ttyS6
|
||||
/dev/tty22 /dev/tty40 /dev/tty59 /dev/ttyS17 /dev/ttyS7
|
||||
/dev/tty23 /dev/tty41 /dev/tty6 /dev/ttyS18 /dev/ttyS8
|
||||
/dev/tty24 /dev/tty42 /dev/tty60 /dev/ttyS19 /dev/ttyS9
|
||||
/dev/tty25 /dev/tty43 /dev/tty61 /dev/ttyS2
|
||||
```
|
||||
|
||||
打开通信，给脚本添加执行权限
|
||||
|
||||
```bash
|
||||
sudo chmod -R 777 /dev/ttyACM0 or sudo chmod -r 777 /dev/ttyUSB0
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/follow_display_gazebo.py
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/slider_control_gazebo.py
|
||||
sudo chmod -R 777 280m5_gripper_gazebo/280m5_gazebo_gripper/scripts/teleop_keyboard_gazebo.py
|
||||
roscor
|
||||
```
|
||||
|
||||
确认好端口后，打开一个终端输入以下命令，注意port改成上一步查询到的值
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper slider.launch _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
接着打开另外一个终端，输入如下命令：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper slider_control_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
同样记得把端口号修改成上一步查询到的端口号。如果运行成功将会看到如下的终端提示：
|
||||
|
||||
```bash
|
||||
('/dev/ttyACM0', 115200)
|
||||
spin ...
|
||||
```
|
||||
|
||||
此时便可通过操控joint_state_publisher_gui的滑块来操控Gazebo或者机械臂模型的位姿了。
|
||||
|
||||
2. Gazebo模型跟随
|
||||
通过如下的命令可以实现Gazebo中的模型跟随实际机械臂的运动而发生位姿的改变，首先运行launch文件：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper follower.launch _port:=/dev/ttyACM0
|
||||
```
|
||||
|
||||
如果程序运行成功，Gazebo界面将成功加载机械臂模型，机械臂模型的所有关节都处于原始位姿，即[0,0,0,0,0,0]. 此后我们打开第二个终端并运行：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper follow_display_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
现在当我们操控实际机械臂的位姿，我们可以看到Gazebo中的机械臂也会跟着一起运动到相同的位姿。
|
||||
|
||||
3. 键盘控制
|
||||
我们还可以使用键盘输入的方式同时操控Gazebo中机械臂模型与实际机械臂的位姿，首先打开一个终端并输入：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
roslaunch 280m5_gazebo_gripper teleop_keyboard.launch _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
同上一部分相同，我们会看到机械臂模型被加载到Gazebo中，并且所有关节都在初始的位姿上，紧接着我们打开另外一个终端并输入：
|
||||
|
||||
```bash
|
||||
source devel/setup.bash
|
||||
rosrun 280m5_gazebo_gripper teleop_keyboard_gazebo.py _port:=/dev/ttyACM0 _baud:=115200
|
||||
```
|
||||
|
||||
如果运行成功，我们将在终端看到如下的输出信息：
|
||||
|
||||
```shell
|
||||
Mycobot_280_m5_gripper Teleop Keyboard Controller
|
||||
---------------------------
|
||||
Movimg options (control the angle of each joint):
|
||||
w: joint2_to_joint1++ s: joint2_to_joint1--
|
||||
e: joint3_to_joint2++ d: joint3_to_joint2--
|
||||
r: joint4_to_joint3++ f: joint4_to_joint3--
|
||||
t: joint5_to_joint4++ g: joint5_to_joint4--
|
||||
y: joint6_to_joint5++ h: joint6_to_joint5--
|
||||
u: joint6output_to_joint6++ j: joint6output_to_joint6--
|
||||
o:open gripper p:close gripper
|
||||
Other:
|
||||
1 - Go to home pose
|
||||
q - Quit
|
||||
```
|
||||
|
||||
根据上面的提示我们可以知道如何操控机械臂运动了，这里我设置每点击一下机械臂与Gazebo中的机械臂模型会运动1角度，可以尝试长按上述键位中的其中一个键来到达某一位姿。
|
||||
734
mycobot_280/280m5_gazebo_gripper/config/2
Normal file
734
mycobot_280/280m5_gazebo_gripper/config/2
Normal file
|
|
@ -0,0 +1,734 @@
|
|||
<sdf version='1.7'>
|
||||
<world name='default'>
|
||||
<light name='sun' type='directional'>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
<pose>0 0 10 0 -0 0</pose>
|
||||
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||
<specular>0.2 0.2 0.2 1</specular>
|
||||
<attenuation>
|
||||
<range>1000</range>
|
||||
<constant>0.9</constant>
|
||||
<linear>0.01</linear>
|
||||
<quadratic>0.001</quadratic>
|
||||
</attenuation>
|
||||
<direction>-0.5 0.1 -0.9</direction>
|
||||
<spot>
|
||||
<inner_angle>0</inner_angle>
|
||||
<outer_angle>0</outer_angle>
|
||||
<falloff>0</falloff>
|
||||
</spot>
|
||||
</light>
|
||||
<model name='ground_plane'>
|
||||
<static>1</static>
|
||||
<link name='link'>
|
||||
<collision name='collision'>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<surface>
|
||||
<contact>
|
||||
<collide_bitmask>65535</collide_bitmask>
|
||||
<ode/>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>100</mu>
|
||||
<mu2>50</mu2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<ode/>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce/>
|
||||
</surface>
|
||||
<max_contacts>10</max_contacts>
|
||||
</collision>
|
||||
<visual name='visual'>
|
||||
<cast_shadows>0</cast_shadows>
|
||||
<geometry>
|
||||
<plane>
|
||||
<normal>0 0 1</normal>
|
||||
<size>100 100</size>
|
||||
</plane>
|
||||
</geometry>
|
||||
<material>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
</material>
|
||||
</visual>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
</model>
|
||||
<gravity>0 0 -9.8</gravity>
|
||||
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
|
||||
<atmosphere type='adiabatic'/>
|
||||
<physics type='ode'>
|
||||
<max_step_size>0.001</max_step_size>
|
||||
<real_time_factor>1</real_time_factor>
|
||||
<real_time_update_rate>1000</real_time_update_rate>
|
||||
</physics>
|
||||
<scene>
|
||||
<ambient>0.4 0.4 0.4 1</ambient>
|
||||
<background>0.7 0.7 0.7 1</background>
|
||||
<shadows>1</shadows>
|
||||
</scene>
|
||||
<wind/>
|
||||
<spherical_coordinates>
|
||||
<surface_model>EARTH_WGS84</surface_model>
|
||||
<latitude_deg>0</latitude_deg>
|
||||
<longitude_deg>0</longitude_deg>
|
||||
<elevation>0</elevation>
|
||||
<heading_deg>0</heading_deg>
|
||||
</spherical_coordinates>
|
||||
<model name='Untitled'>
|
||||
<link name='link_0'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>-0 0 -0 -1.61531 0.023444 -1.57014</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/桌子.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/桌子.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
<geometry>
|
||||
<box>
|
||||
<size>0.05 0.05 0.05</size>
|
||||
</box>
|
||||
</geometry>
|
||||
</link>
|
||||
<static>0</static>
|
||||
<allow_auto_disable>1</allow_auto_disable>
|
||||
<pose>-0.337914 -0.267918 0.21022 0 -0 0</pose>
|
||||
</model>
|
||||
<state world_name='default'>
|
||||
<sim_time>5508 967000000</sim_time>
|
||||
<real_time>57858 546845463</real_time>
|
||||
<wall_time>1749002884 905886341</wall_time>
|
||||
<iterations>5502396</iterations>
|
||||
<model name='Untitled'>
|
||||
<pose>-0.347783 -0.262652 0.215 -0.023413 -0.044529 1.8e-05</pose>
|
||||
<scale>1 1 1</scale>
|
||||
<link name='link_0'>
|
||||
<pose>-0.347783 -0.262652 0.215 -1.5708 2e-06 -1.57012</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>-0.066541 -1.86442 0.124358 2.39003 -0.311467 -0.003264</acceleration>
|
||||
<wrench>-0.066541 -1.86442 0.124358 0 -0 0</wrench>
|
||||
</link>
|
||||
</model>
|
||||
<model name='Untitled_0'>
|
||||
<pose>-0.39788 -0.314647 0.222312 0 -0 0</pose>
|
||||
<scale>1 1 1</scale>
|
||||
<link name='link_0'>
|
||||
<pose>-0.385243 -0.333332 0.221519 1.57851 -0 -1.39915</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
<link name='link_0_clone'>
|
||||
<pose>-0.381654 -0.223326 0.220979 1.60291 -0 -1.39915</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
<link name='link_1'>
|
||||
<pose>-0.248995 -0.241812 0.220455 1.56793 0.005239 1.58595</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
<link name='link_2'>
|
||||
<pose>-0.495218 -0.415801 0.229323 0 -0 0</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
<link name='link_3'>
|
||||
<pose>-0.478291 -0.358964 0.219284 0 -0 0</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
</model>
|
||||
<model name='ground_plane'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<scale>1 1 1</scale>
|
||||
<link name='link'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<velocity>0 0 0 0 -0 0</velocity>
|
||||
<acceleration>0 0 0 0 -0 0</acceleration>
|
||||
<wrench>0 0 0 0 -0 0</wrench>
|
||||
</link>
|
||||
</model>
|
||||
<light name='sun'>
|
||||
<pose>0 0 10 0 -0 0</pose>
|
||||
</light>
|
||||
</state>
|
||||
<gui fullscreen='0'>
|
||||
<camera name='user_camera'>
|
||||
<pose>-0.39406 0.833514 1.03634 0 0.581242 -1.52722</pose>
|
||||
<view_controller>orbit</view_controller>
|
||||
<projection_type>perspective</projection_type>
|
||||
</camera>
|
||||
</gui>
|
||||
<model name='Untitled_0'>
|
||||
<link name='link_0'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>0.012637 -0.018685 -0.000793 1.57851 -0 -1.39915</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/圆柱体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/圆柱体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
<link name='link_0_clone'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>0.016226 0.091321 -0.001333 1.60291 -0 -1.39915</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/圆柱体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/圆柱体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
<link name='link_1'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>0.148885 0.072835 -0.001857 1.56793 0.005239 1.58595</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/盒子.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/盒子.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
<link name='link_2'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>-0.097338 -0.101154 0.007011 0 -0 0</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/球体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/球体.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
<link name='link_3'>
|
||||
<inertial>
|
||||
<mass>1</mass>
|
||||
<inertia>
|
||||
<ixx>0.166667</ixx>
|
||||
<ixy>0</ixy>
|
||||
<ixz>0</ixz>
|
||||
<iyy>0.166667</iyy>
|
||||
<iyz>0</iyz>
|
||||
<izz>0.166667</izz>
|
||||
</inertia>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
</inertial>
|
||||
<pose>-0.080411 -0.044317 -0.003028 0 -0 0</pose>
|
||||
<visual name='visual'>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/长方块.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<material>
|
||||
<lighting>1</lighting>
|
||||
<script>
|
||||
<uri>file://media/materials/scripts/gazebo.material</uri>
|
||||
<name>Gazebo/Grey</name>
|
||||
</script>
|
||||
<shader type='pixel'/>
|
||||
</material>
|
||||
<transparency>0</transparency>
|
||||
<cast_shadows>1</cast_shadows>
|
||||
</visual>
|
||||
<collision name='collision'>
|
||||
<laser_retro>0</laser_retro>
|
||||
<max_contacts>10</max_contacts>
|
||||
<pose>0 0 0 0 -0 0</pose>
|
||||
<geometry>
|
||||
<mesh>
|
||||
<uri>/home/q/Desktop/长方块.dae</uri>
|
||||
<scale>1 1 1</scale>
|
||||
</mesh>
|
||||
</geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>1</mu>
|
||||
<mu2>1</mu2>
|
||||
<fdir1>0 0 0</fdir1>
|
||||
<slip1>0</slip1>
|
||||
<slip2>0</slip2>
|
||||
</ode>
|
||||
<torsional>
|
||||
<coefficient>1</coefficient>
|
||||
<patch_radius>0</patch_radius>
|
||||
<surface_radius>0</surface_radius>
|
||||
<use_patch_radius>1</use_patch_radius>
|
||||
<ode>
|
||||
<slip>0</slip>
|
||||
</ode>
|
||||
</torsional>
|
||||
</friction>
|
||||
<bounce>
|
||||
<restitution_coefficient>0</restitution_coefficient>
|
||||
<threshold>1e+06</threshold>
|
||||
</bounce>
|
||||
<contact>
|
||||
<collide_without_contact>0</collide_without_contact>
|
||||
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
|
||||
<collide_bitmask>1</collide_bitmask>
|
||||
<ode>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0</min_depth>
|
||||
</ode>
|
||||
<bullet>
|
||||
<split_impulse>1</split_impulse>
|
||||
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
|
||||
<soft_cfm>0</soft_cfm>
|
||||
<soft_erp>0.2</soft_erp>
|
||||
<kp>1e+13</kp>
|
||||
<kd>1</kd>
|
||||
</bullet>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<self_collide>0</self_collide>
|
||||
<enable_wind>0</enable_wind>
|
||||
<kinematic>0</kinematic>
|
||||
</link>
|
||||
<static>0</static>
|
||||
<allow_auto_disable>1</allow_auto_disable>
|
||||
<pose>-0.39788 -0.314647 0.222312 0 -0 0</pose>
|
||||
</model>
|
||||
</world>
|
||||
</sdf>
|
||||
|
|
@ -0,0 +1,5 @@
|
|||
cartesian_limits:
|
||||
max_trans_vel: 1
|
||||
max_trans_acc: 2.25
|
||||
max_trans_dec: -5
|
||||
max_rot_vel: 1.57
|
||||
18
mycobot_280/280m5_gazebo_gripper/config/chomp_planning.yaml
Normal file
18
mycobot_280/280m5_gazebo_gripper/config/chomp_planning.yaml
Normal file
|
|
@ -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
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
controller_list:
|
||||
- name: fake_gripper_controller
|
||||
type: $(arg fake_execution_type)
|
||||
joints:
|
||||
- gripper_controller
|
||||
- name: fake_arm_controller
|
||||
type: $(arg fake_execution_type)
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
initial: # Define initial robot poses per group
|
||||
- group: gripper
|
||||
pose: close
|
||||
- group: arm
|
||||
pose: HOME
|
||||
115
mycobot_280/280m5_gazebo_gripper/config/firefighter.srdf
Normal file
115
mycobot_280/280m5_gazebo_gripper/config/firefighter.srdf
Normal file
|
|
@ -0,0 +1,115 @@
|
|||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<!--This does not replace URDF, and is not an extension of URDF.
|
||||
This is a format for representing semantic information about the robot structure.
|
||||
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
|
||||
-->
|
||||
<robot name="firefighter">
|
||||
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
|
||||
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
|
||||
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
|
||||
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
|
||||
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
|
||||
<group name="gripper">
|
||||
<joint name="joint6output_to_gripper_base"/>
|
||||
<joint name="gripper_base_to_gripper_left2"/>
|
||||
<joint name="gripper_base_to_gripper_right2"/>
|
||||
<joint name="gripper_base_to_gripper_right3"/>
|
||||
<joint name="gripper_right3_to_gripper_right1"/>
|
||||
<joint name="gripper_controller"/>
|
||||
<joint name="gripper_left3_to_gripper_left1"/>
|
||||
</group>
|
||||
<group name="arm">
|
||||
<joint name="base_joint"/>
|
||||
<joint name="joint2_to_joint1"/>
|
||||
<joint name="joint3_to_joint2"/>
|
||||
<joint name="joint4_to_joint3"/>
|
||||
<joint name="joint5_to_joint4"/>
|
||||
<joint name="joint6_to_joint5"/>
|
||||
<joint name="joint6output_to_joint6"/>
|
||||
<chain base_link="joint1" tip_link="joint6_flange"/>
|
||||
</group>
|
||||
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
|
||||
<group_state name="HOME" group="arm">
|
||||
<joint name="joint2_to_joint1" value="0" />
|
||||
<joint name="joint3_to_joint2" value="0" />
|
||||
<joint name="joint4_to_joint3" value="0" />
|
||||
<joint name="joint5_to_joint4" value="0" />
|
||||
<joint name="joint6_to_joint5" value="0" />
|
||||
<joint name="joint6output_to_joint6" value="0" />
|
||||
</group_state>
|
||||
<group_state name="FORWARD" group="arm">
|
||||
<joint name="joint2_to_joint1" value="0" />
|
||||
<joint name="joint3_to_joint2" value="1.5706" />
|
||||
<joint name="joint4_to_joint3" value="0" />
|
||||
<joint name="joint5_to_joint4" value="0" />
|
||||
<joint name="joint6_to_joint5" value="0" />
|
||||
<joint name="joint6output_to_joint6" value="0" />
|
||||
</group_state>
|
||||
<group_state name="close" group="gripper">
|
||||
<joint name="gripper_controller" value="-0.5"/>
|
||||
</group_state>
|
||||
<group_state name="open" group="gripper">
|
||||
<joint name="gripper_controller" value="0.1"/>
|
||||
</group_state>
|
||||
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
|
||||
<end_effector name="gripper" parent_link="joint6_flange" group="gripper" parent_group="arm"/>
|
||||
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
|
||||
<passive_joint name="gripper_base_to_gripper_left2"/>
|
||||
<passive_joint name="gripper_base_to_gripper_right2"/>
|
||||
<passive_joint name="gripper_base_to_gripper_right3"/>
|
||||
<passive_joint name="gripper_right3_to_gripper_right1"/>
|
||||
<passive_joint name="gripper_left3_to_gripper_left1"/>
|
||||
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
|
||||
<disable_collisions link1="gripper_base" link2="gripper_left1" reason="Never"/>
|
||||
<disable_collisions link1="gripper_base" link2="gripper_left2" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_base" link2="gripper_left3" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_base" link2="gripper_right1" reason="Never"/>
|
||||
<disable_collisions link1="gripper_base" link2="gripper_right2" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_base" link2="gripper_right3" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_base" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_base" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_base" link2="joint6_flange" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_left1" link2="gripper_left2" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="gripper_left3" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_left1" link2="gripper_right1" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="gripper_right2" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="gripper_right3" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left1" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="gripper_right1" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="gripper_right2" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="gripper_right3" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left2" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="gripper_right1" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="gripper_right2" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="gripper_right3" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_left3" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right1" link2="gripper_right2" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right1" link2="gripper_right3" reason="Adjacent"/>
|
||||
<disable_collisions link1="gripper_right1" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right1" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right1" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right2" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right2" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right2" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right3" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right3" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="gripper_right3" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="joint1" link2="joint2" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint1" link2="joint3" reason="Never"/>
|
||||
<disable_collisions link1="joint2" link2="joint3" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint2" link2="joint5" reason="Never"/>
|
||||
<disable_collisions link1="joint3" link2="joint4" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint4" link2="joint5" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint4" link2="joint6" reason="Never"/>
|
||||
<disable_collisions link1="joint5" link2="joint6" reason="Adjacent"/>
|
||||
<disable_collisions link1="joint5" link2="joint6_flange" reason="Never"/>
|
||||
<disable_collisions link1="joint6" link2="joint6_flange" reason="Adjacent"/>
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,50 @@
|
|||
arm_controller:
|
||||
type: position_controllers/JointTrajectoryController
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
gains:
|
||||
joint2_to_joint1:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
joint3_to_joint2:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
joint4_to_joint3:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
joint5_to_joint4:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
joint6_to_joint5:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
joint6output_to_joint6:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
gripper_controller:
|
||||
type: position_controllers/JointTrajectoryController
|
||||
joints:
|
||||
- gripper_controller
|
||||
gains:
|
||||
gripper_controller:
|
||||
p: 100
|
||||
d: 1
|
||||
i: 1
|
||||
i_clamp: 1
|
||||
548
mycobot_280/280m5_gazebo_gripper/config/gazebo_firefighter.urdf
Normal file
548
mycobot_280/280m5_gazebo_gripper/config/gazebo_firefighter.urdf
Normal file
|
|
@ -0,0 +1,548 @@
|
|||
<?xml version="1.0" ?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
|
||||
<xacro:property name="width" value=".2" />
|
||||
<link name="world" />
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="world" />
|
||||
<child link="joint1" />
|
||||
</joint>
|
||||
<link name="joint1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 1.5708 3.14159-->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0 " rpy=" 0 0 -1.5708" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 1.5708 3.14159-->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0 " rpy=" 0 0 -1.5708" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.06096 " rpy=" 0 0 -1.5708" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.06096 " rpy=" 0 0 -1.5708" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03256 " rpy=" 0 -1.5708 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03256 " rpy=" 0 -1.5708 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint4.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03056 " rpy=" 0 -1.5708 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint4.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03056 " rpy=" 0 -1.5708 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint5.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.03356 " rpy=" -1.5708 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint5.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.03356 " rpy=" -1.5708 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint6.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0 0.00 -0.038 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint6.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0 0.00 -0.038 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint6_flange">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint7.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint7.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_base.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_base.dae"/> -->
|
||||
<cylinder radius="0.02" length="0.01" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.01 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.039 -0.0133 -0.012 " rpy=" 0 0 0" />
|
||||
<!-- <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.008 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.005 -0.0195 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.00 -0.0 -0.001 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.012 0.0025 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.003" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.0 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.039 -0.0133 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.008 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.005 -0.0195 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.001 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.012 0.0025 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.003" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.0" rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint2_to_joint1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.9321" upper="2.9321" velocity="3" />
|
||||
<parent link="joint1" />
|
||||
<child link="joint2" />
|
||||
<origin xyz="0 0 0.13156" rpy="0 0 0" />
|
||||
</joint>
|
||||
<joint name="joint3_to_joint2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.3561" upper="2.3561" velocity="3" />
|
||||
<parent link="joint2" />
|
||||
<child link="joint3" />
|
||||
<origin xyz="0 0 0" rpy="0 1.5708 -1.5708" />
|
||||
</joint>
|
||||
<joint name="joint4_to_joint3" type="revolute">
|
||||
<axis xyz=" 0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.6179" upper="2.6179" velocity="3" />
|
||||
<parent link="joint3" />
|
||||
<child link="joint4" />
|
||||
<origin xyz=" -0.1104 0 0 " rpy="0 0 0" />
|
||||
</joint>
|
||||
<joint name="joint5_to_joint4" type="revolute">
|
||||
<axis xyz=" 0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.5307" upper="2.5307" velocity="3" />
|
||||
<parent link="joint4" />
|
||||
<child link="joint5" />
|
||||
<origin xyz="-0.096 0 0.06462" rpy="0 0 -1.5708" />
|
||||
</joint>
|
||||
<joint name="joint6_to_joint5" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.879793" upper="2.879793" velocity="3" />
|
||||
<parent link="joint5" />
|
||||
<child link="joint6" />
|
||||
<origin xyz="0 -0.07318 0" rpy="1.5708 -1.5708 0" />
|
||||
</joint>
|
||||
<joint name="joint6output_to_joint6" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-3.14" upper="3.14" velocity="3" />
|
||||
<parent link="joint6" />
|
||||
<child link="joint6_flange" />
|
||||
<origin xyz="0 0.0456 0" rpy="-1.5708 0 0" />
|
||||
</joint>
|
||||
<joint name="joint6output_to_gripper_base" type="fixed">
|
||||
<!-- <axis xyz="0 0 1"/>
|
||||
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
|
||||
<parent link="joint6_flange" />
|
||||
<child link="gripper_base" />
|
||||
<origin xyz="0 0 0.034" rpy="1.579 0 0" />
|
||||
</joint>
|
||||
<joint name="gripper_controller" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.78" upper="0.15" velocity="2" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_left3" />
|
||||
<origin xyz="-0.012 0.005 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_left2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.8" upper="0.5" velocity="2" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_left2" />
|
||||
<origin xyz="-0.005 0.027 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_left3_to_gripper_left1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="2" />
|
||||
<parent link="gripper_left3" />
|
||||
<child link="gripper_left1" />
|
||||
<origin xyz="-0.027 0.016 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_right3" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.15" upper="0.7" velocity="2" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_right3" />
|
||||
<origin xyz="0.012 0.005 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_right2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.8" velocity="2" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_right2" />
|
||||
<origin xyz="0.005 0.027 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_right3_to_gripper_right1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="2" />
|
||||
<parent link="gripper_right3" />
|
||||
<child link="gripper_right1" />
|
||||
<origin xyz="0.027 0.016 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
|
||||
</joint>
|
||||
<transmission name="trans_joint2_to_joint1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint2_to_joint1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint2_to_joint1_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint3_to_joint2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint3_to_joint2">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint3_to_joint2_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint4_to_joint3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint4_to_joint3">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint4_to_joint3_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint5_to_joint4">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint5_to_joint4">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint5_to_joint4_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint6_to_joint5">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6_to_joint5">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint6_to_joint5_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint6output_to_joint6">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6output_to_joint6">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint6output_to_joint6_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_controller">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_controller">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_controller_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<gazebo>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_left2">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_left2</mimicJoint>
|
||||
<multiplier>1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_left3_to_gripper_left1">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_left3_to_gripper_left1</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_right3">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_right3</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_right2">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_right2</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_right3_to_gripper_right1">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_right3_to_gripper_right1</mimicJoint>
|
||||
<multiplier>1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<transmission name="trans_gripper_base_to_gripper_left2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_base_to_gripper_left2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_base_to_gripper_left2_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_left3_to_gripper_left1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_left3_to_gripper_left1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_left3_to_gripper_left1_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_base_to_gripper_right3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_base_to_gripper_right3">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_base_to_gripper_right3_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_base_to_gripper_right2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_base_to_gripper_right2">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_base_to_gripper_right2_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_right3_to_gripper_right1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_right3_to_gripper_right1">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_right3_to_gripper_right1_motor">
|
||||
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</robot>
|
||||
|
||||
70
mycobot_280/280m5_gazebo_gripper/config/joint_limits.yaml
Normal file
70
mycobot_280/280m5_gazebo_gripper/config/joint_limits.yaml
Normal file
|
|
@ -0,0 +1,70 @@
|
|||
# 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.5
|
||||
default_acceleration_scaling_factor: 0.5
|
||||
|
||||
# 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:
|
||||
gripper_base_to_gripper_left2:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
gripper_base_to_gripper_right2:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
gripper_base_to_gripper_right3:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
gripper_controller:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
gripper_left3_to_gripper_left1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
gripper_right3_to_gripper_right1:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint2_to_joint1:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint3_to_joint2:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint4_to_joint3:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint5_to_joint4:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint6_to_joint5:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
joint6output_to_joint6:
|
||||
has_velocity_limits: false
|
||||
max_velocity: 0
|
||||
has_acceleration_limits: false
|
||||
max_acceleration: 0
|
||||
7
mycobot_280/280m5_gazebo_gripper/config/kinematics.yaml
Normal file
7
mycobot_280/280m5_gazebo_gripper/config/kinematics.yaml
Normal file
|
|
@ -0,0 +1,7 @@
|
|||
arm:
|
||||
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
|
||||
kinematics_solver_search_resolution: 0.005
|
||||
kinematics_solver_timeout: 0.005
|
||||
goal_joint_tolerance: 0.0001
|
||||
goal_position_tolerance: 0.0001
|
||||
goal_orientation_tolerance: 0.001
|
||||
|
|
@ -0,0 +1,576 @@
|
|||
<?xml version="1.0" ?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
|
||||
<xacro:property name="width" value=".2" />
|
||||
<link name="world" />
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="world" />
|
||||
<child link="joint1" />
|
||||
</joint>
|
||||
<link name="joint1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 1.5708 3.14159-->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0 " rpy=" 0 0 -1.5708" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 1.5708 3.14159-->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0 " rpy=" 0 0 -1.5708" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="1000.0" />
|
||||
<inertia ixx="100" iyy="100" izz="100" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.06096 " rpy=" 0 0 -1.5708" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.06096 " rpy=" 0 0 -1.5708" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="10" />
|
||||
<inertia ixx="1" iyy="1" izz="1" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03256 " rpy=" 0 -1.5708 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03256 " rpy=" 0 -1.5708 0" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="5" />
|
||||
<inertia ixx="0.5" iyy="0.5" izz="0.5" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint4">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint4.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03056 " rpy=" 0 -1.5708 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint4.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.03056 " rpy=" 0 -1.5708 0" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="5" />
|
||||
<inertia ixx="0.5" iyy="0.5" izz="0.5" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint5">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint5.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.03356 " rpy=" -1.5708 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint5.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.03356 " rpy=" -1.5708 0 0" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="2" />
|
||||
<inertia ixx="0.2" iyy="0.2" izz="0.2" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint6">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint6.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0 0.00 -0.038 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint6.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0 0.00 -0.038 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="2" />
|
||||
<inertia ixx="0.2" iyy="0.2" izz="0.2" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="joint6_flange">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint7.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/joint7.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<selfCollide>true</selfCollide>
|
||||
<inertial>
|
||||
<mass value="1" />
|
||||
<inertia ixx="0.1" iyy="0.1" izz="0.1" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_base.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_base.dae"/> -->
|
||||
<cylinder radius="0.02" length="0.01" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.01 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<!--- 0.0 0 -0.04 -->
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.039 -0.0133 -0.012 " rpy=" 0 0 0" />
|
||||
<!-- <origin xyz = "0.0 0 -0.012 " rpy = " 0 0 0"/> -->
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left1.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.008 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.005 -0.0195 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left2.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.00 -0.0 -0.001 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_left3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="0.012 0.0025 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_left3.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.0 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right1">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.039 -0.0133 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right1.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.008 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right2">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.005 -0.0195 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right2.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0.0 -0.001 " rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<link name="gripper_right3">
|
||||
<visual>
|
||||
<geometry>
|
||||
<mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae" />
|
||||
</geometry>
|
||||
<origin xyz="-0.012 0.0025 -0.012 " rpy=" 0 0 0" />
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<!-- <mesh filename="package://mycobot_description/urdf/mycobot_280_m5/gripper_right3.dae"/> -->
|
||||
<cylinder radius="0.0034" length="0.005" />
|
||||
</geometry>
|
||||
<origin xyz="0.0 0 0.0" rpy=" 0 0 0" />
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.0001" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<inertia ixx="0.03" iyy="0.03" izz="0.03" ixy="0.0" ixz="0.0" iyz="0.0" />
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="joint2_to_joint1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.93" upper="2.9321" velocity="1" />
|
||||
<parent link="joint1" />
|
||||
<child link="joint2" />
|
||||
<origin xyz="0 0 0.13156" rpy="0 0 0" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint3_to_joint2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.36" upper="2.3561" velocity="1" />
|
||||
<parent link="joint2" />
|
||||
<child link="joint3" />
|
||||
<origin xyz="0 0 0" rpy="0 1.5708 -1.5708" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint4_to_joint3" type="revolute">
|
||||
<axis xyz=" 0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.62" upper="2.6179" velocity="1" />
|
||||
<parent link="joint3" />
|
||||
<child link="joint4" />
|
||||
<origin xyz=" -0.1104 0 0 " rpy="0 0 0" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint5_to_joint4" type="revolute">
|
||||
<axis xyz=" 0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.53" upper="2.5307" velocity="1" />
|
||||
<parent link="joint4" />
|
||||
<child link="joint5" />
|
||||
<origin xyz="-0.096 0 0.06462" rpy="0 0 -1.5708" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint6_to_joint5" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-2.88" upper="2.8797" velocity="1" />
|
||||
<parent link="joint5" />
|
||||
<child link="joint6" />
|
||||
<origin xyz="0 -0.07318 0" rpy="1.5708 -1.5708 0" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint6output_to_joint6" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-3.14" upper="3.14159" velocity="1" />
|
||||
<parent link="joint6" />
|
||||
<child link="joint6_flange" />
|
||||
<origin xyz="0 0.0456 0" rpy="-1.5708 0 0" />
|
||||
<dynamics damping="0.1" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="joint6output_to_gripper_base" type="fixed">
|
||||
<!-- <axis xyz="0 0 1"/>
|
||||
<limit effort = "1000.0" lower = "-3.14" upper = "3.14159" velocity = "0"/> -->
|
||||
<parent link="joint6_flange" />
|
||||
<child link="gripper_base" />
|
||||
<origin xyz="0 0 0.034" rpy="1.579 0 0" />
|
||||
</joint>
|
||||
<joint name="gripper_controller" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.599" upper="0.15" velocity="1" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_left3" />
|
||||
<origin xyz="-0.012 0.005 0" rpy="0 0 0" />
|
||||
<dynamics damping="20.0" friction="0.1"/>
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_left2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.8" upper="0.5" velocity="1" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_left2" />
|
||||
<origin xyz="-0.005 0.027 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_left3_to_gripper_left1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="1" />
|
||||
<parent link="gripper_left3" />
|
||||
<child link="gripper_left1" />
|
||||
<origin xyz="-0.027 0.016 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_right3" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.15" upper="0.7" velocity="1" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_right3" />
|
||||
<origin xyz="0.012 0.005 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_base_to_gripper_right2" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.8" velocity="1" />
|
||||
<parent link="gripper_base" />
|
||||
<child link="gripper_right2" />
|
||||
<origin xyz="0.005 0.027 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="-1.0" offset="0" />
|
||||
</joint>
|
||||
<joint name="gripper_right3_to_gripper_right1" type="revolute">
|
||||
<axis xyz="0 0 1" />
|
||||
<limit effort="1000.0" lower="-0.5" upper="0.5" velocity="1" />
|
||||
<parent link="gripper_right3" />
|
||||
<child link="gripper_right1" />
|
||||
<origin xyz="0.027 0.016 0" rpy="0 0 0" />
|
||||
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
|
||||
</joint>
|
||||
<transmission name="trans_joint2_to_joint1">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint2_to_joint1">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint2_to_joint1_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint3_to_joint2">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint3_to_joint2">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint3_to_joint2_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint4_to_joint3">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint4_to_joint3">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint4_to_joint3_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint5_to_joint4">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint5_to_joint4">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint5_to_joint4_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint6_to_joint5">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6_to_joint5">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint6_to_joint5_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_joint6output_to_joint6">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="joint6output_to_joint6">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="joint6output_to_joint6_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<transmission name="trans_gripper_controller">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="gripper_controller">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="gripper_controller_motor">
|
||||
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||
<mechanicalReduction>1</mechanicalReduction>
|
||||
</actuator>
|
||||
</transmission>
|
||||
<gazebo>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_left2">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_left2</mimicJoint>
|
||||
<multiplier>1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_left3_to_gripper_left1">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_left3_to_gripper_left1</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_right3">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_right3</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_base_to_gripper_right2">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_base_to_gripper_right2</mimicJoint>
|
||||
<multiplier>-1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin filename="libroboticsgroup_upatras_gazebo_mimic_joint_plugin.so" name="mimic_gripper_right3_to_gripper_right1">
|
||||
<joint>gripper_controller</joint>
|
||||
<mimicJoint>gripper_right3_to_gripper_right1</mimicJoint>
|
||||
<multiplier>1.0</multiplier>
|
||||
<offset>0.0</offset>
|
||||
</plugin>
|
||||
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
|
||||
<robotNamespace>/</robotNamespace>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
<gazebo reference="g_base">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint1">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint2">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint3">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint4">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint5">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint6">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="joint6_flange">
|
||||
<selfCollide>true</selfCollide>
|
||||
</gazebo>
|
||||
<gazebo reference="gripper_left1">
|
||||
<disable_collisions_with>gripper_right1</disable_collisions_with>
|
||||
<surface>
|
||||
<bounce>
|
||||
<restitution_coefficient>0.0</restitution_coefficient>
|
||||
<threshold>1.0</threshold>
|
||||
</bounce>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>20.0</mu>
|
||||
<mu2>20.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="gripper_left2">
|
||||
<disable_collisions_with>gripper_right2</disable_collisions_with>
|
||||
<surface>
|
||||
<bounce>
|
||||
<restitution_coefficient>0.0</restitution_coefficient>
|
||||
<threshold>1.0</threshold>
|
||||
</bounce>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>20.0</mu>
|
||||
<mu2>20.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</gazebo>
|
||||
|
||||
<gazebo reference="gripper_left3">
|
||||
<disable_collisions_with>gripper_right3</disable_collisions_with>
|
||||
<surface>
|
||||
<bounce>
|
||||
<restitution_coefficient>0.0</restitution_coefficient>
|
||||
<threshold>1.0</threshold>
|
||||
</bounce>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>20.0</mu>
|
||||
<mu2>20.0</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
</surface>
|
||||
</gazebo>
|
||||
</robot>
|
||||
|
||||
225
mycobot_280/280m5_gazebo_gripper/config/ompl_planning.yaml
Normal file
225
mycobot_280/280m5_gazebo_gripper/config/ompl_planning.yaml
Normal file
|
|
@ -0,0 +1,225 @@
|
|||
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
|
||||
frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
||||
frontier_node_ratio: 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
|
||||
frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup()
|
||||
frontier_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
|
||||
AITstar:
|
||||
type: geometric::AITstar
|
||||
use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
|
||||
rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
|
||||
samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100
|
||||
use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1
|
||||
find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0
|
||||
set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1
|
||||
ABITstar:
|
||||
type: geometric::ABITstar
|
||||
use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
|
||||
rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
|
||||
samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100
|
||||
use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1
|
||||
prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
|
||||
delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0
|
||||
use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
|
||||
drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
|
||||
stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
|
||||
use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
|
||||
find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0
|
||||
initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000
|
||||
inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
|
||||
truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0
|
||||
BITstar:
|
||||
type: geometric::BITstar
|
||||
use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1
|
||||
rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001
|
||||
samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100
|
||||
use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1
|
||||
prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1
|
||||
delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0
|
||||
use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0
|
||||
drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0
|
||||
stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0
|
||||
use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0
|
||||
find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0
|
||||
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
|
||||
- AITstar
|
||||
- ABITstar
|
||||
- BITstar
|
||||
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
|
||||
- AITstar
|
||||
- ABITstar
|
||||
- BITstar
|
||||
84
mycobot_280/280m5_gazebo_gripper/config/ros_controllers.yaml
Normal file
84
mycobot_280/280m5_gazebo_gripper/config/ros_controllers.yaml
Normal file
|
|
@ -0,0 +1,84 @@
|
|||
# Simulation settings for using moveit_sim_controllers
|
||||
moveit_sim_hw_interface:
|
||||
joint_model_group: mycobot_ros
|
||||
joint_model_group_pose: HOME
|
||||
|
||||
generic_hw_control_loop:
|
||||
loop_hz: 300
|
||||
cycle_time_error_threshold: 0.01
|
||||
# Settings for ros_control hardware interface
|
||||
hardware_interface:
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
sim_control_mode: 0 # 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
|
||||
|
||||
# PID controller settings for each joint
|
||||
gazebo_ros_control/pid_gains:
|
||||
joint2_to_joint1: {p: 100.0, i: 0.01, d: 10.0}
|
||||
joint3_to_joint2: {p: 100.0, i: 0.01, d: 10.0}
|
||||
joint4_to_joint3: {p: 100.0, i: 0.01, d: 10.0}
|
||||
joint5_to_joint4: {p: 100.0, i: 0.01, d: 10.0}
|
||||
joint6_to_joint5: {p: 100.0, i: 0.01, d: 10.0}
|
||||
joint6output_to_joint6: {p: 100.0, i: 0.01, d: 10.0}
|
||||
gripper_controller: {p: 100.0, i: 0.01, d: 10.0}
|
||||
arm_controller:
|
||||
type: position_controllers/JointTrajectoryController
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
|
||||
controller_list:
|
||||
- name: arm_controller
|
||||
action_ns: follow_joint_trajectory
|
||||
type: FollowJointTrajectory
|
||||
default: true
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
|
||||
gripper_controller:
|
||||
type: position_controllers/JointTrajectoryController
|
||||
joints:
|
||||
- gripper_controller
|
||||
|
||||
# 调软 PID：P 不要太大，D 提高一些
|
||||
gains:
|
||||
gripper_controller:
|
||||
p: 20.0 # 降低刚度
|
||||
d: 10.0 # 增加阻尼
|
||||
i: 0.01
|
||||
i_clamp: 0.1
|
||||
|
||||
# 限制抓爪最大速度 & 末点速度
|
||||
constraints:
|
||||
goal_time: 0.5 # 最多等 0.5s 到达
|
||||
stopped_velocity_tolerance: 0.01 # 视为停止的速度阈值
|
||||
gripper_controller:
|
||||
trajectory: 0.02 # 最大轨迹速度 0.05 rad/s
|
||||
goal: 0.01 # 末点速度低于 0.02 rad/s
|
||||
|
||||
controller_list:
|
||||
- name: gripper_controller
|
||||
action_ns: follow_joint_trajectory
|
||||
type: FollowJointTrajectory
|
||||
default: true
|
||||
joints:
|
||||
- gripper_controller
|
||||
2
mycobot_280/280m5_gazebo_gripper/config/sensors_3d.yaml
Normal file
2
mycobot_280/280m5_gazebo_gripper/config/sensors_3d.yaml
Normal file
|
|
@ -0,0 +1,2 @@
|
|||
sensors:
|
||||
[]
|
||||
|
|
@ -0,0 +1,18 @@
|
|||
controller_list:
|
||||
- name: arm_controller
|
||||
action_ns: follow_joint_trajectory
|
||||
type: FollowJointTrajectory
|
||||
default: True
|
||||
joints:
|
||||
- joint2_to_joint1
|
||||
- joint3_to_joint2
|
||||
- joint4_to_joint3
|
||||
- joint5_to_joint4
|
||||
- joint6_to_joint5
|
||||
- joint6output_to_joint6
|
||||
- name: gripper_controller
|
||||
action_ns: follow_joint_trajectory
|
||||
type: FollowJointTrajectory
|
||||
default: True
|
||||
joints:
|
||||
- gripper_controller
|
||||
78
mycobot_280/280m5_gazebo_gripper/config/stomp_planning.yaml
Normal file
78
mycobot_280/280m5_gazebo_gripper/config/stomp_planning.yaml
Normal file
|
|
@ -0,0 +1,78 @@
|
|||
stomp/gripper:
|
||||
group_name: 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]
|
||||
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/arm:
|
||||
group_name: 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
|
||||
|
|
@ -0,0 +1,21 @@
|
|||
<launch>
|
||||
<arg name="start_state_max_bounds_error" default="0.1" />
|
||||
<arg name="jiggle_fraction" default="0.05" />
|
||||
<!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
|
||||
<arg name="planning_adapters"
|
||||
default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
|
||||
default_planner_request_adapters/AddTimeParameterization
|
||||
default_planner_request_adapters/ResolveConstraintFrames
|
||||
default_planner_request_adapters/FixWorkspaceBounds
|
||||
default_planner_request_adapters/FixStartStateBounds
|
||||
default_planner_request_adapters/FixStartStateCollision
|
||||
default_planner_request_adapters/FixStartStatePathConstraints"
|
||||
/>
|
||||
|
||||
<param name="planning_plugin" value="chomp_interface/CHOMPPlanner" />
|
||||
<param name="request_adapters" value="$(arg planning_adapters)" />
|
||||
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
|
||||
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
|
||||
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/chomp_planning.yaml" />
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
<launch>
|
||||
|
||||
<arg name="reset" default="false"/>
|
||||
<!-- If not specified, we'll use a default database location -->
|
||||
<arg name="moveit_warehouse_database_path" default="$(find 280m5_gazebo_gripper)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- Launch the warehouse with the configured database location -->
|
||||
<include file="$(dirname)/warehouse.launch">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg moveit_warehouse_database_path)" />
|
||||
</include>
|
||||
|
||||
<!-- If we want to reset the database, run this node -->
|
||||
<node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
|
||||
|
||||
</launch>
|
||||
66
mycobot_280/280m5_gazebo_gripper/launch/demo.launch
Normal file
66
mycobot_280/280m5_gazebo_gripper/launch/demo.launch
Normal file
|
|
@ -0,0 +1,66 @@
|
|||
<launch>
|
||||
|
||||
<!-- specify the planning pipeline -->
|
||||
<arg name="pipeline" default="ompl" />
|
||||
|
||||
<!-- By default, we do not start a database (it can be large) -->
|
||||
<arg name="db" default="false" />
|
||||
<!-- Allow user to specify database location -->
|
||||
<arg name="db_path" default="$(find 280m5_gazebo_gripper)/default_warehouse_mongo_db" />
|
||||
|
||||
<!-- By default, we are not in debug mode -->
|
||||
<arg name="debug" default="false" />
|
||||
|
||||
<!-- By default, we will load or override the robot_description -->
|
||||
<arg name="load_robot_description" default="true"/>
|
||||
|
||||
<!-- Choose controller manager: fake, simple, or ros_control -->
|
||||
<arg name="moveit_controller_manager" default="fake" />
|
||||
<!-- Set execution mode for fake execution controllers -->
|
||||
<arg name="fake_execution_type" default="interpolate" />
|
||||
|
||||
<!-- By default, hide joint_state_publisher's GUI in 'fake' controller_manager mode -->
|
||||
<arg name="use_gui" default="false" />
|
||||
<arg name="use_rviz" default="true" />
|
||||
|
||||
<!-- If needed, broadcast static tf for robot root -->
|
||||
|
||||
<group if="$(eval arg('moveit_controller_manager') == 'fake')">
|
||||
<!-- We do not have a real robot connected, so publish fake joint states via a joint_state_publisher
|
||||
MoveIt's fake controller's joint states are considered via the 'source_list' parameter -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
<!-- If desired, a GUI version is available allowing to move the simulated robot around manually
|
||||
This corresponds to moving around the real robot without the use of MoveIt. -->
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
|
||||
<rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
|
||||
</node>
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
</group>
|
||||
|
||||
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
|
||||
<include file="$(dirname)/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
|
||||
<arg name="fake_execution_type" value="$(arg fake_execution_type)"/>
|
||||
<arg name="info" value="true"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="pipeline" value="$(arg pipeline)"/>
|
||||
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
|
||||
</include>
|
||||
|
||||
<!-- Run Rviz and load the default config to see the state of the move_group node -->
|
||||
<include file="$(dirname)/moveit_rviz.launch" if="$(arg use_rviz)">
|
||||
<arg name="rviz_config" value="$(dirname)/moveit.rviz"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
</include>
|
||||
|
||||
<!-- If database loading was enabled, start mongodb as well -->
|
||||
<include file="$(dirname)/default_warehouse_db.launch" if="$(arg db)">
|
||||
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
|
||||
</include>
|
||||
|
||||
</launch>
|
||||
28
mycobot_280/280m5_gazebo_gripper/launch/demo_gazebo.launch
Normal file
28
mycobot_280/280m5_gazebo_gripper/launch/demo_gazebo.launch
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MoveIt options -->
|
||||
<arg name="pipeline" default="ompl" doc="Planning pipeline to use with MoveIt"/>
|
||||
|
||||
<!-- Gazebo options -->
|
||||
<arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
|
||||
<arg name="paused" default="false" doc="Start Gazebo paused"/>
|
||||
<arg name="world_name" default="worlds/empty.world" doc="Gazebo world file"/>
|
||||
<arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
|
||||
|
||||
<!-- Launch Gazebo and spawn the robot -->
|
||||
<include file="$(dirname)/gazebo.launch" pass_all_args="true"/>
|
||||
|
||||
<!-- Load gazebo_ros_control plugin -->
|
||||
<node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
|
||||
output="screen" ns="/"
|
||||
args="joint_state_controller" />s
|
||||
<node name="arm_gripper_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
|
||||
output="screen" ns="/"
|
||||
args="arm_controller gripper_controller" />
|
||||
<!-- Launch MoveIt -->
|
||||
<include file="$(dirname)/demo.launch" pass_all_args="true">
|
||||
<!-- robot_description is loaded by gazebo.launch, to enable Gazebo features -->
|
||||
<arg name="load_robot_description" value="false" />
|
||||
<arg name="moveit_controller_manager" value="ros_control" />
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,12 @@
|
|||
<launch>
|
||||
|
||||
<!-- execute the trajectory in 'interpolate' mode or jump to goal position in 'last point' mode -->
|
||||
<arg name="fake_execution_type" default="interpolate" />
|
||||
|
||||
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
|
||||
<param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
|
||||
|
||||
<!-- The rest of the params are specific to this plugin -->
|
||||
<rosparam subst_value="true" file="$(find 280m5_gazebo_gripper)/config/fake_controllers.yaml"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,7 @@
|
|||
<launch>
|
||||
<!-- Define the controller manager plugin to use for trajectory execution -->
|
||||
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
|
||||
|
||||
<!-- loads controller list to the param server -->
|
||||
<rosparam file="$(find 280m5_gazebo_gripper)/config/ros_controllers.yaml"/>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
<launch>
|
||||
|
||||
</launch>
|
||||
44
mycobot_280/280m5_gazebo_gripper/launch/follower.launch
Normal file
44
mycobot_280/280m5_gazebo_gripper/launch/follower.launch
Normal file
|
|
@ -0,0 +1,44 @@
|
|||
<launch>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="gazebo_gui" default="true"/>
|
||||
<arg name="model"
|
||||
default="$(find 280m5_gazebo_gripper)/config/mycobot_280m5_with_gripper_lll.urdf"/>
|
||||
<!-- Optional RViz config -->
|
||||
<!-- <arg name="rvizconfig" default="$(find 280m5_gazebo_gripper)/config/robot.rviz"/> -->
|
||||
|
||||
<!-- 1) 启动 Gazebo empty world -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="worlds/empty.world"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="gui" value="$(arg gazebo_gui)"/>
|
||||
</include>
|
||||
|
||||
<!-- 2) 加载机器人 URDF -->
|
||||
<param name="robot_description" textfile="$(arg model)" />
|
||||
|
||||
<!-- 3) TF & Joint State 发布 -->
|
||||
<node name="robot_state_publisher"
|
||||
pkg="robot_state_publisher" type="robot_state_publisher"/>
|
||||
<node name="joint_state_publisher"
|
||||
pkg="joint_state_publisher" type="joint_state_publisher"/>
|
||||
|
||||
<!-- 4) 把 URDF spawn 到 Gazebo -->
|
||||
<node name="spawn_gazebo_model"
|
||||
pkg="gazebo_ros" type="spawn_model"
|
||||
args="-urdf -param robot_description -model firefighter -x 0 -y 0 -z 0"
|
||||
respawn="false" output="screen"/>
|
||||
|
||||
<!-- 5) 加载 ros_controllers（会启动 controller_spawner）-->
|
||||
<include file="$(find 280m5_gazebo_gripper)/launch/ros_controllers.launch"/>
|
||||
|
||||
<!-- 6) 启动 MoveIt! 核心节点 -->
|
||||
<include file="$(find 280m5_gazebo_gripper)/launch/move_group.launch">
|
||||
<arg name="allow_trajectory_execution" value="true"/>
|
||||
<arg name="fake_execution" value="false"/>
|
||||
<arg name="info" value="true"/>
|
||||
<arg name="debug" value="false"/>
|
||||
<!-- load_robot_description 已经在上面做过 -->
|
||||
<arg name="load_robot_description" value="false"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
||||
36
mycobot_280/280m5_gazebo_gripper/launch/gazebo.launch
Normal file
36
mycobot_280/280m5_gazebo_gripper/launch/gazebo.launch
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- Gazebo options -->
|
||||
<arg name="world_name" value="$(find gazebo_ros)/worlds/empty.world"/>
|
||||
<arg name="gazebo_gui" default="true" doc="Start Gazebo GUI"/>
|
||||
<arg name="paused" default="false" doc="Start Gazebo paused"/>
|
||||
<arg name="world_pose" default="-x 0 -y 0 -z 0 -R 0 -P 0 -Y 0" doc="Pose to spawn the robot at"/>
|
||||
<arg name="initial_joint_positions" default=" -J gripper_controller -0.5" doc="Initial joint configuration of the robot"/>
|
||||
|
||||
<!-- Start Gazebo paused to allow the controllers to pickup the initial pose -->
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(arg world_name)" />
|
||||
</include>
|
||||
|
||||
<!-- Set the robot urdf on the parameter server -->
|
||||
<param name="robot_description" textfile="$(find 280m5_gazebo_gripper)/config/mycobot_280m5_with_gripper_lll.urdf" />
|
||||
|
||||
<!-- Spawn the robot in Gazebo -->
|
||||
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg world_pose) $(arg initial_joint_positions)"
|
||||
respawn="false" output="screen" />
|
||||
|
||||
<!-- Load the controller parameters onto the parameter server -->
|
||||
<rosparam file="$(find 280m5_gazebo_gripper)/config/ros_controllers.yaml" command="load" />
|
||||
|
||||
<!-- Spawn the Gazebo ROS controllers -->
|
||||
<node name="gazebo_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller" />
|
||||
|
||||
<!-- Given the published joint states, publish tf for the robot links -->
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
|
||||
|
||||
<!-- Unpause Gazebo simulation after the robot model and controllers are loaded -->
|
||||
<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
|
||||
<!-- Change the name to "gazebo_ros" or something else to avoid duplication -->
|
||||
<node pkg="gazebo_ros" type="gazebo" name="gazebo_ros" args="--wait $(arg unpause)" />s
|
||||
</launch>
|
||||
|
||||
|
|
@ -0,0 +1,17 @@
|
|||
<launch>
|
||||
<!-- See moveit_ros/visualization/doc/joystick.rst for documentation -->
|
||||
|
||||
<arg name="dev" default="/dev/input/js0" />
|
||||
|
||||
<!-- Launch joy node -->
|
||||
<node pkg="joy" type="joy_node" name="joy">
|
||||
<param name="dev" value="$(arg dev)" /> <!-- Customize this to match the location your joystick is plugged in on-->
|
||||
<param name="deadzone" value="0.2" />
|
||||
<param name="autorepeat_rate" value="40" />
|
||||
<param name="coalesce_interval" value="0.025" />
|
||||
</node>
|
||||
|
||||
<!-- Launch python interface -->
|
||||
<node pkg="moveit_ros_visualization" type="moveit_joy.py" output="screen" name="moveit_joy"/>
|
||||
|
||||
</launch>
|
||||
107
mycobot_280/280m5_gazebo_gripper/launch/move_group.launch
Normal file
107
mycobot_280/280m5_gazebo_gripper/launch/move_group.launch
Normal file
|
|
@ -0,0 +1,107 @@
|
|||
<launch>
|
||||
|
||||
<!-- GDB Debug Option -->
|
||||
<arg name="debug" default="false" />
|
||||
<arg unless="$(arg debug)" name="launch_prefix" value="" />
|
||||
<arg if="$(arg debug)" name="launch_prefix"
|
||||
value="gdb -x $(dirname)/gdb_settings.gdb --ex run --args" />
|
||||
|
||||
<!-- Verbose Mode Option -->
|
||||
<arg name="info" default="$(arg debug)" />
|
||||
<arg unless="$(arg info)" name="command_args" value="" />
|
||||
<arg if="$(arg info)" name="command_args" value="--debug" />
|
||||
|
||||
<!-- move_group settings -->
|
||||
<arg name="pipeline" default="ompl" />
|
||||
<arg name="allow_trajectory_execution" default="true"/>
|
||||
<arg name="moveit_controller_manager" default="simple" />
|
||||
<arg name="fake_execution" default="false"/>
|
||||
<arg name="fake_execution_type" default="interpolate"/>
|
||||
<arg name="max_safe_path_cost" default="1"/>
|
||||
<arg name="publish_monitored_planning_scene" default="true"/>
|
||||
|
||||
<arg name="capabilities" default=""/>
|
||||
<arg name="disable_capabilities" default=""/>
|
||||
<!-- load these non-default MoveGroup capabilities (space seperated) -->
|
||||
<!--
|
||||
<arg name="capabilities" value="
|
||||
a_package/AwsomeMotionPlanningCapability
|
||||
another_package/GraspPlanningPipeline
|
||||
" />
|
||||
-->
|
||||
|
||||
<!-- inhibit these default MoveGroup capabilities (space seperated) -->
|
||||
<!--
|
||||
<arg name="disable_capabilities" value="
|
||||
move_group/MoveGroupKinematicsService
|
||||
move_group/ClearOctomapService
|
||||
" />
|
||||
-->
|
||||
|
||||
<arg name="load_robot_description" default="false" />
|
||||
<!-- load URDF, SRDF and joint_limits configuration -->
|
||||
<include file="$(dirname)/planning_context.launch">
|
||||
<arg name="load_robot_description" value="$(arg load_robot_description)" />
|
||||
</include>
|
||||
|
||||
<!-- Planning Pipelines -->
|
||||
<group ns="move_group/planning_pipelines">
|
||||
|
||||
<!-- OMPL -->
|
||||
<include file="$(dirname)/planning_pipeline.launch.xml">
|
||||
<arg name="pipeline" value="ompl" />
|
||||
</include>
|
||||
|
||||
<!-- CHOMP -->
|
||||
<include file="$(dirname)/planning_pipeline.launch.xml">
|
||||
<arg name="pipeline" value="chomp" />
|
||||
</include>
|
||||
|
||||
<!-- Pilz Industrial Motion -->
|
||||
<include file="$(dirname)/planning_pipeline.launch.xml">
|
||||
<arg name="pipeline" value="pilz_industrial_motion_planner" />
|
||||
</include>
|
||||
|
||||
<!-- Support custom planning pipeline -->
|
||||
<include if="$(eval arg('pipeline') not in ['ompl', 'chomp', 'pilz_industrial_motion_planner'])"
|
||||
file="$(dirname)/planning_pipeline.launch.xml">
|
||||
<arg name="pipeline" value="$(arg pipeline)" />
|
||||
</include>
|
||||
</group>
|
||||
|
||||
<!-- Trajectory Execution Functionality -->
|
||||
<include ns="move_group" file="$(dirname)/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
|
||||
<arg name="moveit_manage_controllers" value="true" />
|
||||
<arg name="moveit_controller_manager" value="firefighter" unless="$(arg fake_execution)"/>
|
||||
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
|
||||
<arg name="fake_execution_type" value="$(arg fake_execution_type)" />
|
||||
</include>
|
||||
|
||||
<!-- Sensors Functionality -->
|
||||
<include ns="move_group" file="$(dirname)/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
|
||||
<arg name="moveit_sensor_manager" value="firefighter" />
|
||||
</include>
|
||||
|
||||
<!-- Start the actual move_group node/action server -->
|
||||
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
|
||||
<!-- Set the display variable, in case OpenGL code is used internally -->
|
||||
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
|
||||
|
||||
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
|
||||
<param name="sense_for_plan/max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
|
||||
<param name="default_planning_pipeline" value="$(arg pipeline)" />
|
||||
<param name="capabilities" value="$(arg capabilities)" />
|
||||
<param name="disable_capabilities" value="$(arg disable_capabilities)" />
|
||||
|
||||
<!-- do not copy dynamics information from /joint_states to internal robot monitoring
|
||||
default to false, because almost nothing in move_group relies on this information -->
|
||||
<param name="monitor_dynamics" value="false" />
|
||||
|
||||
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
|
||||
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
|
||||
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
|
||||
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
|
||||
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
131
mycobot_280/280m5_gazebo_gripper/launch/moveit.rviz
Normal file
131
mycobot_280/280m5_gazebo_gripper/launch/moveit.rviz
Normal file
|
|
@ -0,0 +1,131 @@
|
|||
Panels:
|
||||
- Class: rviz/Displays
|
||||
Help Height: 84
|
||||
Name: Displays
|
||||
Property Tree Widget:
|
||||
Expanded:
|
||||
- /MotionPlanning1
|
||||
Splitter Ratio: 0.5
|
||||
Tree Height: 226
|
||||
- Class: rviz/Help
|
||||
Name: Help
|
||||
- Class: rviz/Views
|
||||
Expanded:
|
||||
- /Current View1
|
||||
Name: Views
|
||||
Splitter Ratio: 0.5
|
||||
Visualization Manager:
|
||||
Class: ""
|
||||
Displays:
|
||||
- Alpha: 0.5
|
||||
Cell Size: 1
|
||||
Class: rviz/Grid
|
||||
Color: 160; 160; 164
|
||||
Enabled: true
|
||||
Line Style:
|
||||
Line Width: 0.03
|
||||
Value: Lines
|
||||
Name: Grid
|
||||
Normal Cell Count: 0
|
||||
Offset:
|
||||
X: 0
|
||||
Y: 0
|
||||
Z: 0
|
||||
Plane: XY
|
||||
Plane Cell Count: 10
|
||||
Reference Frame: <Fixed Frame>
|
||||
Value: true
|
||||
- Class: moveit_rviz_plugin/MotionPlanning
|
||||
Enabled: true
|
||||
Name: MotionPlanning
|
||||
Planned Path:
|
||||
Links: ~
|
||||
Loop Animation: true
|
||||
Robot Alpha: 0.5
|
||||
Show Robot Collision: false
|
||||
Show Robot Visual: true
|
||||
Show Trail: false
|
||||
State Display Time: 0.05 s
|
||||
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
|
||||
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
|
||||
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
|
||||
Show Scene Geometry: true
|
||||
Voxel Coloring: Z-Axis
|
||||
Voxel Rendering: Occupied Voxels
|
||||
Scene Robot:
|
||||
Attached Body Color: 150; 50; 150
|
||||
Links: ~
|
||||
Robot Alpha: 0.5
|
||||
Show Scene Robot: true
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 48; 48; 48
|
||||
Fixed Frame: world
|
||||
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.0
|
||||
Enable Stereo Rendering:
|
||||
Stereo Eye Separation: 0.06
|
||||
Stereo Focal Distance: 1
|
||||
Swap Stereo Eyes: false
|
||||
Value: false
|
||||
Field of View: 0.75
|
||||
Focal Point:
|
||||
X: -0.1
|
||||
Y: 0.25
|
||||
Z: 0.30
|
||||
Focal Shape Fixed Size: true
|
||||
Focal Shape Size: 0.05
|
||||
Invert Z Axis: false
|
||||
Name: Current View
|
||||
Near Clip Distance: 0.01
|
||||
Pitch: 0.5
|
||||
Target Frame: world
|
||||
Yaw: -0.6232355833053589
|
||||
Saved: ~
|
||||
Window Geometry:
|
||||
Displays:
|
||||
collapsed: false
|
||||
Height: 848
|
||||
Help:
|
||||
collapsed: false
|
||||
Hide Left Dock: false
|
||||
Hide Right Dock: false
|
||||
MotionPlanning:
|
||||
collapsed: false
|
||||
MotionPlanning - Trajectory Slider:
|
||||
collapsed: false
|
||||
QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||
Views:
|
||||
collapsed: false
|
||||
Width: 1291
|
||||
X: 454
|
||||
Y: 25
|
||||
15
mycobot_280/280m5_gazebo_gripper/launch/moveit_rviz.launch
Normal file
15
mycobot_280/280m5_gazebo_gripper/launch/moveit_rviz.launch
Normal file
|
|
@ -0,0 +1,15 @@
|
|||
<launch>
|
||||
|
||||
<arg name="debug" default="false" />
|
||||
<arg unless="$(arg debug)" name="launch_prefix" value="" />
|
||||
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
|
||||
|
||||
<arg name="rviz_config" default="" />
|
||||
<arg if="$(eval rviz_config=='')" name="command_args" value="" />
|
||||
<arg unless="$(eval rviz_config=='')" name="command_args" value="-d $(arg rviz_config)" />
|
||||
|
||||
<node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
|
||||
args="$(arg command_args)" output="screen">
|
||||
</node>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
<launch>
|
||||
<!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
|
||||
<include file="$(find 280m5_gazebo_gripper)/launch/ompl_planning_pipeline.launch.xml">
|
||||
<arg name="planning_adapters"
|
||||
default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
|
||||
default_planner_request_adapters/AddTimeParameterization
|
||||
default_planner_request_adapters/FixWorkspaceBounds
|
||||
default_planner_request_adapters/FixStartStateBounds
|
||||
default_planner_request_adapters/FixStartStateCollision
|
||||
default_planner_request_adapters/FixStartStatePathConstraints
|
||||
chomp/OptimizerAdapter"
|
||||
/>
|
||||
</include>
|
||||
|
||||
<!-- load chomp config -->
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/chomp_planning.yaml" />
|
||||
|
||||
<!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
|
||||
<param name="trajectory_initialization_method" value="fillTrajectory"/>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,24 @@
|
|||
<launch>
|
||||
|
||||
<!-- The request adapters (plugins) used when planning with OMPL. ORDER MATTERS! -->
|
||||
<arg name="planning_adapters"
|
||||
default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed
|
||||
default_planner_request_adapters/AddTimeParameterization
|
||||
default_planner_request_adapters/ResolveConstraintFrames
|
||||
default_planner_request_adapters/FixWorkspaceBounds
|
||||
default_planner_request_adapters/FixStartStateBounds
|
||||
default_planner_request_adapters/FixStartStateCollision
|
||||
default_planner_request_adapters/FixStartStatePathConstraints"
|
||||
/>
|
||||
|
||||
<arg name="start_state_max_bounds_error" default="0.1" />
|
||||
<arg name="jiggle_fraction" default="0.05" />
|
||||
|
||||
<param name="planning_plugin" value="ompl_interface/OMPLPlanner" />
|
||||
<param name="request_adapters" value="$(arg planning_adapters)" />
|
||||
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
|
||||
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
|
||||
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/ompl_planning.yaml"/>
|
||||
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,15 @@
|
|||
<launch>
|
||||
|
||||
<!-- The request adapters (plugins) used when planning. ORDER MATTERS! -->
|
||||
<arg name="planning_adapters" default="" />
|
||||
|
||||
<param name="planning_plugin" value="pilz_industrial_motion_planner::CommandPlanner" />
|
||||
<param name="request_adapters" value="$(arg planning_adapters)" />
|
||||
|
||||
<!-- Define default planner (for all groups) -->
|
||||
<param name="default_planner_config" value="PTP" />
|
||||
|
||||
<!-- MoveGroup capabilities to load for this pipeline, append sequence capability -->
|
||||
<param name="capabilities" value="pilz_industrial_motion_planner/MoveGroupSequenceAction
|
||||
pilz_industrial_motion_planner/MoveGroupSequenceService" />
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,26 @@
|
|||
<launch>
|
||||
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
|
||||
<arg name="load_robot_description" default="false"/>
|
||||
|
||||
<!-- The name of the parameter under which the URDF is loaded -->
|
||||
<arg name="robot_description" default="robot_description"/>
|
||||
|
||||
<!-- Load universal robot description format (URDF) -->
|
||||
<param if="$(arg load_robot_description)" name="$(arg robot_description)" textfile="$(find 280m5_gazebo_gripper)/config/mycobot_280m5_with_gripper_lll.urdf"/>
|
||||
|
||||
<!-- The semantic description that corresponds to the URDF -->
|
||||
<param name="$(arg robot_description)_semantic" textfile="$(find 280m5_gazebo_gripper)/config/firefighter.srdf" />
|
||||
|
||||
<!-- Load updated joint limits (override information from URDF) -->
|
||||
<group ns="$(arg robot_description)_planning">
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/joint_limits.yaml"/>
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/cartesian_limits.yaml"/>
|
||||
</group>
|
||||
|
||||
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
|
||||
<group ns="$(arg robot_description)_kinematics">
|
||||
<rosparam command="load" file="$(find 280m5_gazebo_gripper)/config/kinematics.yaml"/>
|
||||
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
Some files were not shown because too many files have changed in this diff Show more
Loading…
Add table
Reference in a new issue