kenaug74/mycobot_ros
1
0
Fork 0
mirror of https://github.com/elephantrobotics/mycobot_ros.git synced 2026-07-03 02:27:04 +00:00
Code Issues Projects Releases Packages Wiki Activity Actions

fix(320-gazebo):Fixed compilation failures; temporarily removed the 320 Gazebo package.

Browse source
This commit is contained in:
wangWking 2026-04-14 18:05:41 +08:00
parent 96572cde7f
commit 640c4ea666
165 changed files with 0 additions and 26548 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

BIN
mycobot_320m5_gazebo/mycobot_320_description/urdf/d435i.dae → mycobot_320m5_gazebo.zip
View file

Binary file not shown.

44
mycobot_320m5_gazebo/README.md
View file

@ -1,44 +0,0 @@
### 本地 1.操作流程
#### 1.1 安装前提
要使用此包，需先安装[Python api](https://github.com/elephantrobotics/pymycobot.git)库。
```bash
pip install pymycobot --user
ros1 noetic
```
#### 1.2安装320m5对应的ros镜像版本
#### 1.3 包的下载与安装
下载包到你的ros工作空间中
```bash
$ cd ~/catkin_ws/src
$ git clone https://github.com/jiaweilong66/mycobot_320m5_gazebo.git
$ cd ~/catkin_ws/mycobot_320m5_gazebo
$ git checkout visual
$ cd ..
$ catkin_make
$ source devel/setup.bash
```
#### 1.4 启动步骤
#第一步：
启动仿真环境：
```
source ./devel/setup.bash
roslaunch demo_moveit demo_gazebo.launch
```
#（无视觉抓取）第二步启动程序：
```
source ./devel/setup.bash
rosrun vision_control demo.py
```
#（含视觉抓取）第二步启动程序：
```
source ./devel/setup.bash
rosrun vision_control robot.py
```

11
mycobot_320m5_gazebo/arm_moveit/.setup_assistant
View file

@ -1,11 +0,0 @@
moveit_setup_assistant_config:
URDF:
package: mycobot_description
relative_path: urdf/gazebo_firefighter.urdf
xacro_args: ""
SRDF:
relative_path: config/firefighter.srdf
CONFIG:
author_name: lijie
author_email: 429229407@qq.com
generated_timestamp: 1754905752

10
mycobot_320m5_gazebo/arm_moveit/CMakeLists.txt
View file

@ -1,10 +0,0 @@
cmake_minimum_required(VERSION 3.1.3)
project(arm_moveit)
find_package(catkin REQUIRED)
catkin_package()
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

5
mycobot_320m5_gazebo/arm_moveit/config/cartesian_limits.yaml
View file

@ -1,5 +0,0 @@
cartesian_limits:
max_trans_vel: 1
max_trans_acc: 2.25
max_trans_dec: -5
max_rot_vel: 1.57

18
mycobot_320m5_gazebo/arm_moveit/config/chomp_planning.yaml
View file

@ -1,18 +0,0 @@
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

19
mycobot_320m5_gazebo/arm_moveit/config/fake_controllers.yaml
View file

@ -1,19 +0,0 @@
controller_list:
- 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
- name: fake_gripper_controller
type: $(arg fake_execution_type)
joints:
- gripper_controller
initial: # Define initial robot poses per group
# - group: arm
# pose: home
[]

130
mycobot_320m5_gazebo/arm_moveit/config/firefighter.srdf
View file

@ -1,130 +0,0 @@
<?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="arm">
<chain base_link="base" tip_link="link6"/>
</group>
<group name="gripper">
<joint name="joint6output_to_gripper_connect"/>
<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>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand" parent_link="link6" 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="base" link2="gripper_base" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left2" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right2" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="base" link2="link1" reason="Adjacent"/>
<disable_collisions link1="base" link2="link2" reason="Never"/>
<disable_collisions link1="base" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_base" link2="gripper_connection" reason="Adjacent"/>
<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="link1" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left2" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_right2" reason="Default"/>
<disable_collisions link1="gripper_connection" link2="gripper_right3" reason="Default"/>
<disable_collisions link1="gripper_connection" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link6" 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="link1" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Default"/>
<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="Default"/>
<disable_collisions link1="gripper_left2" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link6" 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="Default"/>
<disable_collisions link1="gripper_left3" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link6" 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="link1" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link6" reason="Never"/>
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
<disable_collisions link1="link1" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
<disable_collisions link1="link2" link2="link4" reason="Never"/>
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
<disable_collisions link1="link3" link2="link5" reason="Never"/>
<disable_collisions link1="link3" link2="link6" reason="Never"/>
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
<disable_collisions link1="link4" link2="link6" reason="Never"/>
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
</robot>

4
mycobot_320m5_gazebo/arm_moveit/config/gazebo_controllers.yaml
View file

@ -1,4 +0,0 @@
# Publish joint_states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

644
mycobot_320m5_gazebo/arm_moveit/config/gazebo_firefighter.urdf
View file

@ -1,644 +0,0 @@
<?xml version="1.0" ?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">
<!-- Properties -->
<xacro:property name="width" value=".2" />
<!-- World Link -->
<link name="world" />
<!-- Base Joint -->
<joint name="base_joint" type="fixed">
<parent link="world" />
<child link="base" />
<origin xyz="0 0.05 0.215" rpy="0 0 0" />
</joint>
<!-- Base Link -->
<link name="base">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/base.dae" />
</geometry>
<origin xyz="0.0 0 0" rpy="0 0 1.5708" />
</visual>
<collision>
<origin xyz="0.0 0.0 0.05" rpy="0 0 1.5708" />
<geometry>
<cylinder length="0.05" radius="0.053" />
</geometry>
</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" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 1 -->
<link name="link1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link1.dae" />
</geometry>
<origin xyz="0.0 0.0 -0.078" rpy="0 0 1.5708" />
</visual>
<collision>
<origin xyz="0.0 0 -0.05" rpy="0 0 1.5708" />
<geometry>
<cylinder length="0.1" radius="0.033" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="10.0" />
<inertia ixx="1" iyy="1" izz="1" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 2 -->
<link name="link2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link2.dae" />
</geometry>
<origin xyz="0.0 0.0 -0.04" rpy="0 1.5708 0" />
</visual>
<collision>
<origin xyz="0.065 0 0" rpy="0 1.5708 0" />
<geometry>
<cylinder length="0.15" radius="0.024" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="5.0" />
<inertia ixx="0.5" iyy="0.5" izz="0.5" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 3 -->
<link name="link3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link3.dae" />
</geometry>
<origin xyz="0.0 0.0 0.039" rpy="1.5708 1.5708 1.5708" />
</visual>
<collision>
<origin xyz="0.062 0.0 -0.01" rpy="1.5708 1.5708 1.5708" />
<geometry>
<cylinder length="0.14" radius="0.0205" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="5.0" />
<inertia ixx="0.5" iyy="0.5" izz="0.5" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 4 -->
<link name="link4">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link4.dae" />
</geometry>
<origin xyz="0.0 -0.0024 -0.05" rpy="1.5708 1.5708 0" />
</visual>
<collision>
<origin xyz="0.0 -0.0024 -0.05" rpy="0 0 1.5708" />
<geometry>
<cylinder length="0.09" radius="0.024" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" iyy="0.2" izz="0.2" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 5 -->
<link name="link5">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link5.dae" />
</geometry>
<origin xyz="0.0 0.0 -0.05" rpy="0 0 -1.5708" />
</visual>
<collision>
<origin xyz="0.0 0.0 -0.05" rpy="0 0 -1.5708" />
<geometry>
<cylinder length="0.1" radius="0.019" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" iyy="0.2" izz="0.2" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 6 -->
<link name="link6">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link6.dae" />
</geometry>
<origin xyz="0.0 0.0 -0.0115" rpy="1.5708 1.5708 0" />
</visual>
<collision>
<origin xyz="0.0 0.0 0" rpy="0 0 1.5708" />
<geometry>
<cylinder length="0.015" radius="0.026" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" iyy="0.2" izz="0.2" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Gripper Connection -->
<link name="gripper_connection">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/mygripper_f100_connection.dae" />
</geometry>
<origin xyz="-0.03 0.014 0" rpy="0 0 -1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/mygripper_f100_connection.dae" />
</geometry>
<origin xyz="-0.03 0.014 0" rpy="0 0 -1.5708" />
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="1.0" />
<inertia ixx="0.1" iyy="0.1" izz="0.1" ixy="0.0" ixz="0.0" iyz="0.0" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Gripper Base -->
<link name="gripper_base">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_base.dae" />
</geometry>
<origin xyz="-0.107 0.012 0.0" rpy="1.5708 0 1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_base.dae" />
</geometry>
<origin xyz="-0.107 0.012 0.0" rpy="1.5708 0 1.5708" />
</collision>
<inertial>
<mass value="0.05" />
<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>
<!-- Gripper Left 1 -->
<link name="gripper_left1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left1.dae" />
</geometry>
<origin xyz="-0.047 -0.013 0.044" rpy="1.5708 0 1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left1.dae" />
</geometry>
<origin xyz="-0.047 -0.013 0.044" rpy="1.5708 0 1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<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>
<!-- Gripper Left 2 -->
<link name="gripper_left2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left2.dae" />
</geometry>
<origin xyz="-0.0 0.03 0.004" rpy="3.14159 0 1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left2.dae" />
</geometry>
<origin xyz="-0.0 0.03 0.004" rpy="3.14159 0 1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<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>
<!-- Gripper Left 3 -->
<link name="gripper_left3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left3.dae" />
</geometry>
<origin xyz="0 -0.03 0.012" rpy="1.5708 3.14159 1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left3.dae" />
</geometry>
<origin xyz="0 -0.03 0.012" rpy="1.5708 3.14159 1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<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>
<!-- Gripper Right 1 -->
<link name="gripper_right1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right1.dae" />
</geometry>
<origin xyz="0.047 -0.013 -0.046" rpy="1.5708 3.14 -1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right1.dae" />
</geometry>
<origin xyz="0.047 -0.013 -0.046" rpy="1.5708 3.14 -1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<origin xyz="0 0 0" rpy="0 0 0" />
<inertia ixx="0.03" iyy="0.03" izz="0.033" ixy="0.0" ixz="0.0" iyz="0.0" />
</inertial>
</link>
<!-- Gripper Right 2 -->
<link name="gripper_right2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right2.dae" />
</geometry>
<origin xyz="0 0.03 -0.005" rpy="0 0 1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right2.dae" />
</geometry>
<origin xyz="0 0.03 -0.005" rpy="0 0 1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<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>
<!-- Gripper Right 3 -->
<link name="gripper_right3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right3.dae" />
</geometry>
<origin xyz="-0.014 -0.029 0.004" rpy="3.14 0 -1.5708" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right3.dae" />
</geometry>
<origin xyz="-0.014 -0.029 0.004" rpy="3.14 0 -1.5708" />
</collision>
<inertial>
<mass value="0.005" />
<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>
<!-- Joints -->
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="-2.93" upper="2.93" velocity="1" />
<parent link="base" />
<child link="link1" />
<origin xyz="0 0 0.173" 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.35" upper="2.35" velocity="1" />
<parent link="link1" />
<child link="link2" />
<origin xyz="0 -0.086 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.53" upper="2.53" velocity="1" />
<parent link="link2" />
<child link="link3" />
<origin xyz="0.13635 0 -0.086" 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.58" upper="2.58" velocity="1" />
<parent link="link3" />
<child link="link4" />
<origin xyz="0.1195 0 0.082" rpy="0 0 1.57080" />
<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.93" upper="2.93" velocity="1" />
<parent link="link4" />
<child link="link5" />
<origin xyz="0 -0.09415 0.0" rpy="1.5708 0 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.14" velocity="1" />
<parent link="link5" />
<child link="link6" />
<origin xyz="0 0.055 0.0" rpy="-1.5708 0 0" />
<dynamics damping="0.1" friction="0.1" />
</joint>
<joint name="joint6output_to_gripper_connect" type="fixed">
<parent link="link6" />
<child link="gripper_connection" />
<origin xyz="0 0.0 0.01" rpy="0 0 0" />
<dynamics damping="0.1" friction="0.1" />
</joint>
<joint name="joint6output_to_gripper_base" type="fixed">
<parent link="gripper_connection" />
<child link="gripper_base" />
<origin xyz="0 -0.02 0.0364" rpy="1.5708 0 0" />
</joint>
<!-- Gripper Joints -->
<joint name="gripper_controller" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="0" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_left3" />
<origin xyz="-0.018 0.016 -0.02" rpy="0 0 0" />
<dynamics damping="20" friction="0.1" />
</joint>
<joint name="gripper_base_to_gripper_left2" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_left2" />
<origin xyz="-0.0445 -0.004 -0.02" 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="-1" upper="1" velocity="1" />
<parent link="gripper_left3" />
<child link="gripper_left1" />
<origin xyz="0.01 0.06 0.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="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_right3" />
<origin xyz="0.018 0.015 -0.0212" 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="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_right2" />
<origin xyz="0.0442 -0.004 -0.02" 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="-1" upper="1" velocity="1" />
<parent link="gripper_right3" />
<child link="gripper_right1" />
<origin xyz="-0.01 0.061 0.001" rpy="0 0 0" />
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
</joint>
<!-- Transmissions -->
<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 Plugins -->
<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="base">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link1">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link2">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link3">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link4">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link5">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link6">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="gripper_connection">
<selfCollide>true</selfCollide>
</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> -->
<gazebo>
<gripper name="gazebo_gripper">
<grasp_check>
<attach_steps>2</attach_steps>
<detach_steps>2</detach_steps>
<min_contact_count>3</min_contact_count>
</grasp_check>
<gripper_link>gripper_right1</gripper_link>
<gripper_link>gripper_left1</gripper_link>
<palm_link>gripper_connection</palm_link>
</gripper>
</gazebo>
<gazebo>
<plugin filename="libgazebo_grasp_fix.so" name="gazebo_grasp_fix">
<arm>
<arm_name>ur5_gripper</arm_name>
<palm_link>link6</palm_link>
<gripper_link>gripper_left1</gripper_link>
<gripper_link>gripper_right1</gripper_link>
<gripper_link>gripper_right2</gripper_link>
<gripper_link>gripper_left2</gripper_link>
</arm>
<forces_angle_tolerance>150</forces_angle_tolerance>
<update_rate>130</update_rate>
<grip_count_threshold>2</grip_count_threshold>
<max_grip_count>8</max_grip_count>
<release_tolerance>0.1</release_tolerance>
<disable_collisions_on_attach>true</disable_collisions_on_attach>
<contact_topic>__default_topic__</contact_topic>
</plugin>
</gazebo>
</robot>

70
mycobot_320m5_gazebo/arm_moveit/config/joint_limits.yaml
View file

@ -1,70 +0,0 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
gripper_base_to_gripper_left2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_base_to_gripper_right2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_base_to_gripper_right3:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_controller:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_left3_to_gripper_left1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_right3_to_gripper_right1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint2_to_joint1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint3_to_joint2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint4_to_joint3:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint5_to_joint4:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint6_to_joint5:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint6output_to_joint6:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0

7
mycobot_320m5_gazebo/arm_moveit/config/kinematics.yaml
View file

@ -1,7 +0,0 @@
arm:
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
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

226
mycobot_320m5_gazebo/arm_moveit/config/ompl_planning.yaml
View file

@ -1,226 +0,0 @@
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
arm:
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
projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2)
longest_valid_segment_fraction: 0.005
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

50
mycobot_320m5_gazebo/arm_moveit/config/ros_controllers.yaml
View file

@ -1,50 +0,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
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

2
mycobot_320m5_gazebo/arm_moveit/config/sensors_3d.yaml
View file

@ -1,2 +0,0 @@
sensors:
[]

18
mycobot_320m5_gazebo/arm_moveit/config/simple_moveit_controllers.yaml
View file

@ -1,18 +0,0 @@
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_320m5_gazebo/arm_moveit/config/stomp_planning.yaml
View file

@ -1,78 +0,0 @@
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
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

21
mycobot_320m5_gazebo/arm_moveit/launch/chomp_planning_pipeline.launch.xml
View file

@ -1,21 +0,0 @@
<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 arm_moveit)/config/chomp_planning.yaml" />
</launch>

15
mycobot_320m5_gazebo/arm_moveit/launch/default_warehouse_db.launch
View file

@ -1,15 +0,0 @@
<launch>
<arg name="reset" default="false"/>
<!-- If not specified, we'll use a default database location -->
<arg name="moveit_warehouse_database_path" default="$(find arm_moveit)/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_320m5_gazebo/arm_moveit/launch/demo.launch
View file

@ -1,66 +0,0 @@
<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 arm_moveit)/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>

21
mycobot_320m5_gazebo/arm_moveit/launch/demo_gazebo.launch
View file

@ -1,21 +0,0 @@
<?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"/>
<!-- 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>

12
mycobot_320m5_gazebo/arm_moveit/launch/fake_moveit_controller_manager.launch.xml
View file

@ -1,12 +0,0 @@
<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 arm_moveit)/config/fake_controllers.yaml"/>
</launch>

3
mycobot_320m5_gazebo/arm_moveit/launch/firefighter_moveit_sensor_manager.launch.xml
View file

@ -1,3 +0,0 @@
<launch>
</launch>

36
mycobot_320m5_gazebo/arm_moveit/launch/gazebo.launch
View file

@ -1,36 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- 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"/>
<arg name="initial_joint_positions" default="" 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" pass_all_args="true">
<arg name="world_name" value="$(find mycobot_320_description)/world/test.world" />
<arg name="paused" value="true"/>
</include>
<!-- Set the robot urdf on the parameter server -->
<param name="robot_description" textfile="$(find arm_moveit)/config/gazebo_firefighter.urdf" />
<!-- Unpause the simulation after loading the robot model -->
<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<!-- Spawn the robot in Gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
respawn="false" output="screen" />
<!-- Load the controller parameters onto the parameter server -->
<rosparam file="$(find arm_moveit)/config/gazebo_controllers.yaml" />
<include file="$(dirname)/ros_controllers.launch"/>
<!-- 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" />
</launch>

17
mycobot_320m5_gazebo/arm_moveit/launch/joystick_control.launch
View file

@ -1,17 +0,0 @@
<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>

105
mycobot_320m5_gazebo/arm_moveit/launch/move_group.launch
View file

@ -1,105 +0,0 @@
<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_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="$(arg moveit_controller_manager)" />
<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_320m5_gazebo/arm_moveit/launch/moveit.rviz
View file

@ -1,131 +0,0 @@
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_320m5_gazebo/arm_moveit/launch/moveit_rviz.launch
View file

@ -1,15 +0,0 @@
<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>

20
mycobot_320m5_gazebo/arm_moveit/launch/ompl-chomp_planning_pipeline.launch.xml
View file

@ -1,20 +0,0 @@
<launch>
<!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
<include file="$(find arm_moveit)/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 arm_moveit)/config/chomp_planning.yaml" />
<!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
<param name="trajectory_initialization_method" value="fillTrajectory"/>
</launch>

24
mycobot_320m5_gazebo/arm_moveit/launch/ompl_planning_pipeline.launch.xml
View file

@ -1,24 +0,0 @@
<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 arm_moveit)/config/ompl_planning.yaml"/>
</launch>

15
mycobot_320m5_gazebo/arm_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
View file

@ -1,15 +0,0 @@
<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>

27
mycobot_320m5_gazebo/arm_moveit/launch/planning_context.launch
View file

@ -1,27 +0,0 @@
<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 mycobot_320_description)/urdf/gazebo_firefighter.urdf"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find arm_moveit)/config/firefighter.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find arm_moveit)/config/joint_limits.yaml"/>
<rosparam command="load" file="$(find arm_moveit)/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 arm_moveit)/config/kinematics.yaml"/>
</group>
</launch>

10
mycobot_320m5_gazebo/arm_moveit/launch/planning_pipeline.launch.xml
View file

@ -1,10 +0,0 @@
<launch>
<!-- This file makes it easy to include different planning pipelines;
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
<include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
</launch>

4
mycobot_320m5_gazebo/arm_moveit/launch/ros_control_moveit_controller_manager.launch.xml
View file

@ -1,4 +0,0 @@
<launch>
<!-- Define MoveIt controller manager plugin -->
<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
</launch>

11
mycobot_320m5_gazebo/arm_moveit/launch/ros_controllers.launch
View file

@ -1,11 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find arm_moveit)/config/ros_controllers.yaml" command="load"/>
<!-- Load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="arm_controller gripper_controller "/>
</launch>

21
mycobot_320m5_gazebo/arm_moveit/launch/run_benchmark_ompl.launch
View file

@ -1,21 +0,0 @@
<launch>
<!-- This argument must specify the list of .cfg files to process for benchmarking -->
<arg name="cfg" />
<!-- Load URDF -->
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- Start the database -->
<include file="$(dirname)/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
</include>
<!-- Start Benchmark Executable -->
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
<rosparam command="load" file="$(find arm_moveit)/config/ompl_planning.yaml"/>
</node>
</launch>

17
mycobot_320m5_gazebo/arm_moveit/launch/sensor_manager.launch.xml
View file

@ -1,17 +0,0 @@
<launch>
<!-- This file makes it easy to include the settings for sensor managers -->
<!-- Params for 3D sensors config -->
<rosparam command="load" file="$(find arm_moveit)/config/sensors_3d.yaml" />
<!-- Params for the octomap monitor -->
<!-- <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
<param name="octomap_resolution" type="double" value="0.025" />
<param name="max_range" type="double" value="5.0" />
<!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
<arg name="moveit_sensor_manager" default="firefighter" />
<include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

16
mycobot_320m5_gazebo/arm_moveit/launch/setup_assistant.launch
View file

@ -1,16 +0,0 @@
<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
<launch>
<!-- Debug Info -->
<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" />
<!-- Run -->
<node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
args="--config_pkg=arm_moveit"
launch-prefix="$(arg launch_prefix)"
required="true"
output="screen" />
</launch>

8
mycobot_320m5_gazebo/arm_moveit/launch/simple_moveit_controller_manager.launch.xml
View file

@ -1,8 +0,0 @@
<launch>
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<!-- Load controller list to the parameter server -->
<rosparam file="$(find arm_moveit)/config/simple_moveit_controllers.yaml" />
<rosparam file="$(find arm_moveit)/config/ros_controllers.yaml" />
</launch>

23
mycobot_320m5_gazebo/arm_moveit/launch/stomp_planning_pipeline.launch.xml
View file

@ -1,23 +0,0 @@
<launch>
<!-- Stomp Plugin for MoveIt -->
<arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
<arg name="start_state_max_bounds_error" value="0.1" />
<arg name="jiggle_fraction" value="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/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints" />
<param name="planning_plugin" value="$(arg planning_plugin)" />
<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 arm_moveit)/config/stomp_planning.yaml"/>
</launch>

23
mycobot_320m5_gazebo/arm_moveit/launch/trajectory_execution.launch.xml
View file

@ -1,23 +0,0 @@
<launch>
<!-- This file summarizes all settings required for trajectory execution -->
<!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" />
<arg name="fake_execution_type" default="interpolate" />
<!-- Flag indicating whether MoveIt is allowed to load/unload or switch controllers -->
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
<param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
<!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
<include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
</launch>

15
mycobot_320m5_gazebo/arm_moveit/launch/warehouse.launch
View file

@ -1,15 +0,0 @@
<launch>
<!-- The path to the database must be specified -->
<arg name="moveit_warehouse_database_path" />
<!-- Load warehouse parameters -->
<include file="$(dirname)/warehouse_settings.launch.xml" />
<!-- Run the DB server -->
<node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
<param name="overwrite" value="false"/>
<param name="database_path" value="$(arg moveit_warehouse_database_path)" />
</node>
</launch>

16
mycobot_320m5_gazebo/arm_moveit/launch/warehouse_settings.launch.xml
View file

@ -1,16 +0,0 @@
<launch>
<!-- Set the parameters for the warehouse and run the mongodb server. -->
<!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
<arg name="moveit_warehouse_port" default="33829" />
<!-- The default DB host for moveit -->
<arg name="moveit_warehouse_host" default="localhost" />
<!-- Set parameters for the warehouse -->
<param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
<param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
<param name="warehouse_exec" value="mongod" />
<param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
</launch>

42
mycobot_320m5_gazebo/arm_moveit/package.xml
View file

@ -1,42 +0,0 @@
<package>
<name>arm_moveit</name>
<version>0.3.0</version>
<description>
An automatically generated package with all the configuration and launch files for using the firefighter with the MoveIt Motion Planning Framework
</description>
<author email="429229407@qq.com">lijie</author>
<maintainer email="429229407@qq.com">lijie</maintainer>
<license>BSD</license>
<url type="website">http://moveit.ros.org/</url>
<url type="bugtracker">https://github.com/moveit/moveit/issues</url>
<url type="repository">https://github.com/moveit/moveit</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_kinematics</run_depend>
<run_depend>moveit_planners</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_setup_assistant</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>joint_state_publisher_gui</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>rviz</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>xacro</run_depend>
<!-- The next 2 packages are required for the gazebo simulation.
We don't include them by default to prevent installing gazebo and all its dependencies. -->
<!-- <run_depend>joint_trajectory_controller</run_depend> -->
<!-- <run_depend>gazebo_ros_control</run_depend> -->
<!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
<!-- <run_depend>warehouse_ros_mongo</run_depend> -->
<run_depend>mycobot_320_description</run_depend>
</package>

11
mycobot_320m5_gazebo/demo_moveit/.setup_assistant
View file

@ -1,11 +0,0 @@
moveit_setup_assistant_config:
URDF:
package: mycobot_description
relative_path: urdf/robot_robotiq85_gripper.xacro
xacro_args: ""
SRDF:
relative_path: config/ur5.srdf
CONFIG:
author_name: lijie
author_email: 429229407@qq.com
generated_timestamp: 1754993128

10
mycobot_320m5_gazebo/demo_moveit/CMakeLists.txt
View file

@ -1,10 +0,0 @@
cmake_minimum_required(VERSION 3.1.3)
project(demo_moveit)
find_package(catkin REQUIRED)
catkin_package()
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

5
mycobot_320m5_gazebo/demo_moveit/config/cartesian_limits.yaml
View file

@ -1,5 +0,0 @@
cartesian_limits:
max_trans_vel: 1
max_trans_acc: 2.25
max_trans_dec: -5
max_rot_vel: 1.57

18
mycobot_320m5_gazebo/demo_moveit/config/chomp_planning.yaml
View file

@ -1,18 +0,0 @@
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

17
mycobot_320m5_gazebo/demo_moveit/config/fake_controllers.yaml
View file

@ -1,17 +0,0 @@
controller_list:
- 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
- name: fake_gripper_controller
type: $(arg fake_execution_type)
joints:
- gripper_controller
initial: # Define initial robot poses per group
# - group: arm
# pose: home

4
mycobot_320m5_gazebo/demo_moveit/config/gazebo_controllers.yaml
View file

@ -1,4 +0,0 @@
# Publish joint_states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50

794
mycobot_320m5_gazebo/demo_moveit/config/gazebo_ur5.urdf
View file

@ -1,794 +0,0 @@
<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- | This document was autogenerated by xacro from /home/lijie/桌面/demo_test2/src/mycobot_320_description/urdf/robot_robotiq85_gripper.xacro | -->
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
<!-- =================================================================================== -->
<robot name="ur5">
<!-- World Link -->
<link name="world" />
<!-- Base Joint -->
<joint name="base_joint" type="fixed">
<parent link="world" />
<child link="base" />
<origin xyz="0 0.05 0.215" rpy="0 0 0" />
</joint>
<!-- Base Link -->
<link name="base">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/base.dae" />
</geometry>
<origin rpy="0 0 1.5708" xyz="0.0 0 0" />
</visual>
<collision>
<origin rpy="0 0 1.5708" xyz="0.0 0.0 0.05" />
<geometry>
<cylinder length="0.05" radius="0.053" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="1000.0" />
<inertia ixx="100" ixy="0.0" ixz="0.0" iyy="100" iyz="0.0" izz="100" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 1 -->
<link name="link1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link1.dae" />
</geometry>
<origin rpy="0 0 1.5708" xyz="0.0 0.0 -0.078" />
</visual>
<collision>
<origin rpy="0 0 1.5708" xyz="0.0 0 -0.05" />
<geometry>
<cylinder length="0.1" radius="0.033" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="10.0" />
<inertia ixx="1" ixy="0.0" ixz="0.0" iyy="1" iyz="0.0" izz="1" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 2 -->
<link name="link2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link2.dae" />
</geometry>
<origin rpy="0 1.5708 0" xyz="0.0 0.0 -0.04" />
</visual>
<collision>
<origin rpy="0 1.5708 0" xyz="0.065 0 0" />
<geometry>
<cylinder length="0.15" radius="0.024" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="5.0" />
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 3 -->
<link name="link3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link3.dae" />
</geometry>
<origin rpy="1.5708 1.5708 1.5708" xyz="0.0 0.0 0.039" />
</visual>
<collision>
<origin rpy="1.5708 1.5708 1.5708" xyz="0.062 0.0 -0.01" />
<geometry>
<cylinder length="0.14" radius="0.0205" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="5.0" />
<inertia ixx="0.5" ixy="0.0" ixz="0.0" iyy="0.5" iyz="0.0" izz="0.5" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 4 -->
<link name="link4">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link4.dae" />
</geometry>
<origin rpy="1.5708 1.5708 0" xyz="0.0 -0.0024 -0.05" />
</visual>
<collision>
<origin rpy="0 0 1.5708" xyz="0.0 -0.0024 -0.05" />
<geometry>
<cylinder length="0.09" radius="0.024" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 5 -->
<link name="link5">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link5.dae" />
</geometry>
<origin rpy="0 0 -1.5708" xyz="0.0 0.0 -0.05" />
</visual>
<collision>
<origin rpy="0 0 -1.5708" xyz="0.0 0.0 -0.05" />
<geometry>
<cylinder length="0.1" radius="0.019" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Link 6 -->
<link name="link6">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/link6.dae" />
</geometry>
<origin rpy="1.5708 1.5708 0" xyz="0.0 0.0 -0.0115" />
</visual>
<collision>
<origin rpy="0 0 1.5708" xyz="0.0 0.0 0" />
<geometry>
<cylinder length="0.015" radius="0.026" />
</geometry>
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="2.0" />
<inertia ixx="0.2" ixy="0.0" ixz="0.0" iyy="0.2" iyz="0.0" izz="0.2" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Gripper Connection -->
<link name="gripper_connection">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/mygripper_f100_connection.dae" />
</geometry>
<origin rpy="0 0 -1.5708" xyz="-0.03 0.014 0" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/mygripper_f100_connection.dae" />
</geometry>
<origin rpy="0 0 -1.5708" xyz="-0.03 0.014 0" />
</collision>
<selfCollide>true</selfCollide>
<inertial>
<mass value="1.0" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
<origin xyz="0 0 0" rpy="0 0 0" />
</inertial>
</link>
<!-- Gripper Base -->
<link name="gripper_base">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_base.dae" />
</geometry>
<origin rpy="1.5708 0 1.5708" xyz="-0.107 0.012 0.0" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_base.dae" />
</geometry>
<origin rpy="1.5708 0 1.5708" xyz="-0.107 0.012 0.0" />
</collision>
<inertial>
<mass value="0.05" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Gripper Left 1 -->
<link name="gripper_left1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left1.dae" />
</geometry>
<origin rpy="1.5708 0 1.5708" xyz="-0.047 -0.013 0.044" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left1.dae" />
</geometry>
<origin rpy="1.5708 0 1.5708" xyz="-0.047 -0.013 0.044" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Gripper Left 2 -->
<link name="gripper_left2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left2.dae" />
</geometry>
<origin rpy="3.14159 0 1.5708" xyz="-0.0 0.03 0.004" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left2.dae" />
</geometry>
<origin rpy="3.14159 0 1.5708" xyz="-0.0 0.03 0.004" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Gripper Left 3 -->
<link name="gripper_left3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left3.dae" />
</geometry>
<origin rpy="1.5708 3.14159 1.5708" xyz="0 -0.03 0.012" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_left3.dae" />
</geometry>
<origin rpy="1.5708 3.14159 1.5708" xyz="0 -0.03 0.012" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Gripper Right 1 -->
<link name="gripper_right1">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right1.dae" />
</geometry>
<origin rpy="1.5708 3.14 -1.5708" xyz="0.047 -0.013 -0.046" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right1.dae" />
</geometry>
<origin rpy="1.5708 3.14 -1.5708" xyz="0.047 -0.013 -0.046" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.033" />
</inertial>
</link>
<!-- Gripper Right 2 -->
<link name="gripper_right2">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right2.dae" />
</geometry>
<origin rpy="0 0 1.5708" xyz="0 0.03 -0.005" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right2.dae" />
</geometry>
<origin rpy="0 0 1.5708" xyz="0 0.03 -0.005" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Gripper Right 3 -->
<link name="gripper_right3">
<visual>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right3.dae" />
</geometry>
<origin rpy="3.14 0 -1.5708" xyz="-0.014 -0.029 0.004" />
</visual>
<collision>
<geometry>
<mesh filename="package://mycobot_320_description/urdf/mycobot_320_m5_2022/force_control_gripper/pro_gripper_right3.dae" />
</geometry>
<origin rpy="3.14 0 -1.5708" xyz="-0.014 -0.029 0.004" />
</collision>
<inertial>
<mass value="0.005" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="0.03" ixy="0.0" ixz="0.0" iyy="0.03" iyz="0.0" izz="0.03" />
</inertial>
</link>
<!-- Joints -->
<joint name="joint2_to_joint1" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="-2.93" upper="2.93" velocity="1" />
<parent link="base" />
<child link="link1" />
<origin rpy="0 0 0" xyz="0 0 0.173" />
<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.35" upper="2.35" velocity="1" />
<parent link="link1" />
<child link="link2" />
<origin rpy="0 -1.5708 1.5708" xyz="0 -0.086 0" />
<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.53" upper="2.53" velocity="1" />
<parent link="link2" />
<child link="link3" />
<origin rpy="0 0 0" xyz="0.13635 0 -0.086" />
<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.58" upper="2.58" velocity="1" />
<parent link="link3" />
<child link="link4" />
<origin rpy="0 0 1.57080" xyz="0.1195 0 0.082" />
<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.93" upper="2.93" velocity="1" />
<parent link="link4" />
<child link="link5" />
<origin rpy="1.5708 0 0" xyz="0 -0.09415 0.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.14" velocity="1" />
<parent link="link5" />
<child link="link6" />
<origin rpy="-1.5708 0 0" xyz="0 0.055 0.0" />
<dynamics damping="0.1" friction="0.1" />
</joint>
<joint name="joint6output_to_gripper_connect" type="fixed">
<parent link="link6" />
<child link="gripper_connection" />
<origin rpy="0 0 0" xyz="0 0.0 0.01" />
<dynamics damping="0.1" friction="0.1" />
</joint>
<joint name="joint6output_to_gripper_base" type="fixed">
<parent link="gripper_connection" />
<child link="gripper_base" />
<origin rpy="1.5708 0 0" xyz="0 -0.02 0.0364" />
</joint>
<!-- Gripper Joints -->
<joint name="gripper_controller" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="0" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_left3" />
<origin rpy="0 0 0" xyz="-0.018 0.016 -0.02" />
<dynamics damping="20" friction="0.1" />
</joint>
<joint name="gripper_base_to_gripper_left2" type="revolute">
<axis xyz="0 0 1" />
<limit effort="1000.0" lower="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_left2" />
<origin rpy="0 0 0" xyz="-0.0445 -0.004 -0.02" />
<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="-1" upper="1" velocity="1" />
<parent link="gripper_left3" />
<child link="gripper_left1" />
<origin rpy="0 0 0" xyz="0.01 0.06 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="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_right3" />
<origin rpy="0 0 0" xyz="0.018 0.015 -0.0212" />
<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="-1" upper="1" velocity="1" />
<parent link="gripper_base" />
<child link="gripper_right2" />
<origin rpy="0 0 0" xyz="0.0442 -0.004 -0.02" />
<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="-1" upper="1" velocity="1" />
<parent link="gripper_right3" />
<child link="gripper_right1" />
<origin rpy="0 0 0" xyz="-0.01 0.061 0.001" />
<mimic joint="gripper_controller" multiplier="1.0" offset="0" />
</joint>
<!-- Transmissions -->
<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 Plugins -->
<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 filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/</robotNamespace>
</plugin>
</gazebo>
<gazebo reference="base">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link1">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link2">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link3">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link4">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link5">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="link6">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo reference="gripper_connection">
<selfCollide>true</selfCollide>
</gazebo>
<gazebo>
<gripper name="gazebo_gripper">
<grasp_check>
<attach_steps>2</attach_steps>
<detach_steps>2</detach_steps>
<min_contact_count>3</min_contact_count>
</grasp_check>
<gripper_link>gripper_right1</gripper_link>
<gripper_link>gripper_left1</gripper_link>
<palm_link>gripper_connection</palm_link>
</gripper>
</gazebo>
<gazebo>
<plugin filename="libgazebo_grasp_fix.so" name="gazebo_grasp_fix">
<arm>
<arm_name>ur5_gripper</arm_name>
<palm_link>link6</palm_link>
<gripper_link>gripper_left1</gripper_link>
<gripper_link>gripper_right1</gripper_link>
<gripper_link>gripper_right2</gripper_link>
<gripper_link>gripper_left2</gripper_link>
</arm>
<forces_angle_tolerance>150</forces_angle_tolerance>
<update_rate>130</update_rate>
<grip_count_threshold>2</grip_count_threshold>
<max_grip_count>8</max_grip_count>
<release_tolerance>0.005</release_tolerance>
<disable_collisions_on_attach>true</disable_collisions_on_attach>
<contact_topic>__default_topic__</contact_topic>
</plugin>
</gazebo>
<link name="camera_base_link">
<collision>
<!-- <origin xyz="0 0 0" rpy="0 0 0"/> -->
<origin rpy="1.570796327 0 1.570796327" xyz="0 0 0" />
<geometry>
<!-- <box size="0.01 0.01 0.01"/> -->
<mesh filename="package://mycobot_320_description/urdf/d435i.dae" />
</geometry>
</collision>
<visual>
<!-- <origin xyz="0 0 0" rpy="0 0 0"/> -->
<origin rpy="1.570796327 0 1.570796327" xyz="0 0 0" />
<geometry>
<!-- <box size="0.05 0.1 0.02"/> -->
<!-- <mesh filename="package://kinect_v2/meshes/kinect2.STL" /> -->
<!-- <mesh filename="package://robot_sim_demo/models/meshes/xtion_pro_camera.dae" /> -->
<!-- <mesh filename="package://robot_sim_demo/models/meshes/kinect.dae" /> -->
<mesh filename="package://mycobot_320_description/urdf/d435i.dae" />
</geometry>
<material name="">
<color rgba="0.0 0.0 0.5 1" />
</material>
</visual>
<inertial>
<mass value="1e-5" />
<origin rpy="0 0 0" xyz="0 0 0" />
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<link name="cam_rgb_link" />
<link name="camera_rgb_optical_frame" />
<link name="camera_ir_link" />
<link name="camera_ir_optical_frame" />
<joint name="camera_rgb_joint" type="fixed">
<origin rpy="0 0 0" xyz="-0.01 0.0175 0" />
<parent link="camera_base_link" />
<child link="cam_rgb_link" />
</joint>
<joint name="camera_rgb_optical_frame_joint" type="fixed">
<origin rpy="-1.570796327 0 -1.570796327" xyz="0 0 0" />
<parent link="cam_rgb_link" />
<child link="camera_rgb_optical_frame" />
</joint>
<joint name="camera_ir_joint" type="fixed">
<origin rpy="0 0 0" xyz="0.00 0.015 0.00" />
<parent link="cam_rgb_link" />
<child link="camera_ir_link" />
</joint>
<joint name="camera_ir_optical_frame_joint" type="fixed">
<origin rpy="-1.570796327 0 -1.570796327" xyz="0 0 0" />
<parent link="camera_ir_link" />
<child link="camera_ir_optical_frame" />
</joint>
<gazebo reference="camera_base_link">
<turnGravityOff>true</turnGravityOff>
</gazebo>
<gazebo reference="cam_rgb_link">
<turnGravityOff>true</turnGravityOff>
</gazebo>
<gazebo reference="camera_ir_link">
<turnGravityOff>true</turnGravityOff>
</gazebo>
<gazebo reference="camera_ir_optical_frame">
<turnGravityOff>true</turnGravityOff>
</gazebo>
<gazebo reference="cam_rgb_link">
<sensor name="cam_rgb_sensor" type="depth">
<always_on>true</always_on>
<update_rate>30</update_rate>
<camera>
<horizontal_fov>0.9948376737666667</horizontal_fov>
<image>
<format>B8G8R8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.01</near>
<far>5</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_openni_kinect.so" name="cam_rgb_link_controller">
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<cameraName>camera/rgb</cameraName>
<imageTopicName>/camera/rgb/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/rgb/camera_info</cameraInfoTopicName>
<frameName>camera_rgb_optical_frame</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>320</Cx>
<Cy>240</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
<gazebo reference="camera_ir_link">
<sensor name="cam_ir_sensor" type="depth">
<always_on>true</always_on>
<update_rate>30</update_rate>
<camera>
<horizontal_fov>0.9948376737666667</horizontal_fov>
<image>
<format>L8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.01</near>
<far>5</far>
</clip>
</camera>
<plugin filename="libgazebo_ros_openni_kinect.so" name="camera_ir_link_controller">
<!-- <baseline>0.2</baseline> -->
<alwaysOn>true</alwaysOn>
<updateRate>30</updateRate>
<cameraName>camera/ir</cameraName>
<imageTopicName>/camera/ir/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/ir/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<frameName>camera_ir_optical_frame</frameName>
<pointCloudCutoff>0.1</pointCloudCutoff>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>320</Cx>
<Cy>240</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
</gazebo>
<joint name="camera_arm" type="fixed">
<origin rpy="1.5707963 -1.5707963 0 " xyz="0.0 0.05 0.045" />
<parent link="link6" />
<child link="camera_base_link" />
</joint>
<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_320m5_gazebo/demo_moveit/config/joint_limits.yaml
View file

@ -1,70 +0,0 @@
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
# For beginners, we downscale velocity and acceleration limits.
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
default_velocity_scaling_factor: 0.1
default_acceleration_scaling_factor: 0.1
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
joint_limits:
gripper_base_to_gripper_left2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_base_to_gripper_right2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_base_to_gripper_right3:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_controller:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_left3_to_gripper_left1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
gripper_right3_to_gripper_right1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint2_to_joint1:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint3_to_joint2:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint4_to_joint3:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint5_to_joint4:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint6_to_joint5:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0
joint6output_to_joint6:
has_velocity_limits: true
max_velocity: 1
has_acceleration_limits: false
max_acceleration: 0

7
mycobot_320m5_gazebo/demo_moveit/config/kinematics.yaml
View file

@ -1,7 +0,0 @@
arm:
kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin
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

227
mycobot_320m5_gazebo/demo_moveit/config/ompl_planning.yaml
View file

@ -1,227 +0,0 @@
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
arm:
default_planner_config: RRTConnect
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
projection_evaluator: joints(joint2_to_joint1,joint3_to_joint2)
longest_valid_segment_fraction: 0.005
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

50
mycobot_320m5_gazebo/demo_moveit/config/ros_controllers.yaml
View file

@ -1,50 +0,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
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

2
mycobot_320m5_gazebo/demo_moveit/config/sensors_3d.yaml
View file

@ -1,2 +0,0 @@
sensors:
[]

18
mycobot_320m5_gazebo/demo_moveit/config/simple_moveit_controllers.yaml
View file

@ -1,18 +0,0 @@
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_320m5_gazebo/demo_moveit/config/stomp_planning.yaml
View file

@ -1,78 +0,0 @@
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
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

146
mycobot_320m5_gazebo/demo_moveit/config/ur5.srdf
View file

@ -1,146 +0,0 @@
<?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="ur5">
<!--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="arm">
<chain base_link="base" tip_link="link6"/>
</group>
<group name="gripper">
<joint name="joint6output_to_gripper_connect"/>
<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>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="hand" parent_link="link6" 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="base" link2="camera_base_link" reason="Never"/>
<disable_collisions link1="base" link2="gripper_base" reason="Never"/>
<disable_collisions link1="base" link2="gripper_connection" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left2" reason="Never"/>
<disable_collisions link1="base" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right2" reason="Never"/>
<disable_collisions link1="base" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="base" link2="link1" reason="Adjacent"/>
<disable_collisions link1="base" link2="link2" reason="Never"/>
<disable_collisions link1="base" link2="link5" reason="Never"/>
<!-- <disable_collisions link1="camera_base_link" link2="gripper_base" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_connection" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_left2" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_right2" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link1" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link2" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link3" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link4" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link5" reason="Never"/>
<disable_collisions link1="camera_base_link" link2="link6" reason="Adjacent"/> -->
<disable_collisions link1="gripper_base" link2="gripper_connection" reason="Adjacent"/>
<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="link1" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_base" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left2" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="gripper_right2" reason="Default"/>
<disable_collisions link1="gripper_connection" link2="gripper_right3" reason="Default"/>
<disable_collisions link1="gripper_connection" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_connection" link2="link6" 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="link1" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Default"/>
<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="Default"/>
<disable_collisions link1="gripper_left2" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="link6" 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="Default"/>
<disable_collisions link1="gripper_left3" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="link6" 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="link1" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="link6" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link1" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link2" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link3" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link4" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link5" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="link6" reason="Never"/>
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
<disable_collisions link1="link1" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
<disable_collisions link1="link2" link2="link4" reason="Never"/>
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
<disable_collisions link1="link3" link2="link5" reason="Never"/>
<disable_collisions link1="link3" link2="link6" reason="Never"/>
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
<disable_collisions link1="link4" link2="link6" reason="Never"/>
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
</robot>

21
mycobot_320m5_gazebo/demo_moveit/launch/chomp_planning_pipeline.launch.xml
View file

@ -1,21 +0,0 @@
<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 demo_moveit)/config/chomp_planning.yaml" />
</launch>

15
mycobot_320m5_gazebo/demo_moveit/launch/default_warehouse_db.launch
View file

@ -1,15 +0,0 @@
<launch>
<arg name="reset" default="false"/>
<!-- If not specified, we'll use a default database location -->
<arg name="moveit_warehouse_database_path" default="$(find demo_moveit)/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_320m5_gazebo/demo_moveit/launch/demo.launch
View file

@ -1,66 +0,0 @@
<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 demo_moveit)/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>

21
mycobot_320m5_gazebo/demo_moveit/launch/demo_gazebo.launch
View file

@ -1,21 +0,0 @@
<?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"/>
<!-- 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>

12
mycobot_320m5_gazebo/demo_moveit/launch/fake_moveit_controller_manager.launch.xml
View file

@ -1,12 +0,0 @@
<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 demo_moveit)/config/fake_controllers.yaml"/>
</launch>

36
mycobot_320m5_gazebo/demo_moveit/launch/gazebo.launch
View file

@ -1,36 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- 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"/>
<arg name="initial_joint_positions" default="" 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" pass_all_args="true">
<arg name="world_name" value="$(find mycobot_320_description)/world/test.world" />
<arg name="paused" value="true"/>
</include>
<!-- Set the robot urdf on the parameter server -->
<param name="robot_description" textfile="$(find demo_moveit)/config/gazebo_ur5.urdf" />
<!-- Unpause the simulation after loading the robot model -->
<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<!-- Spawn the robot in Gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot $(arg unpause) $(arg world_pose) $(arg initial_joint_positions)"
respawn="false" output="screen" />
<!-- Load the controller parameters onto the parameter server -->
<rosparam file="$(find demo_moveit)/config/gazebo_controllers.yaml" />
<include file="$(dirname)/ros_controllers.launch"/>
<!-- 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" />
</launch>

17
mycobot_320m5_gazebo/demo_moveit/launch/joystick_control.launch
View file

@ -1,17 +0,0 @@
<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>

105
mycobot_320m5_gazebo/demo_moveit/launch/move_group.launch
View file

@ -1,105 +0,0 @@
<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_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="$(arg moveit_controller_manager)" />
<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="ur5" />
</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_320m5_gazebo/demo_moveit/launch/moveit.rviz
View file

@ -1,131 +0,0 @@
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_320m5_gazebo/demo_moveit/launch/moveit_rviz.launch
View file

@ -1,15 +0,0 @@
<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>

20
mycobot_320m5_gazebo/demo_moveit/launch/ompl-chomp_planning_pipeline.launch.xml
View file

@ -1,20 +0,0 @@
<launch>
<!-- load OMPL planning pipeline, but add the CHOMP planning adapter. -->
<include file="$(find demo_moveit)/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 demo_moveit)/config/chomp_planning.yaml" />
<!-- override trajectory_initialization_method: Use OMPL-generated trajectory -->
<param name="trajectory_initialization_method" value="fillTrajectory"/>
</launch>

24
mycobot_320m5_gazebo/demo_moveit/launch/ompl_planning_pipeline.launch.xml
View file

@ -1,24 +0,0 @@
<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 demo_moveit)/config/ompl_planning.yaml"/>
</launch>

15
mycobot_320m5_gazebo/demo_moveit/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml
View file

@ -1,15 +0,0 @@
<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>

27
mycobot_320m5_gazebo/demo_moveit/launch/planning_context.launch
View file

@ -1,27 +0,0 @@
<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)" command="xacro '$(find mycobot_320_description)/urdf/robot_robotiq85_gripper.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find demo_moveit)/config/ur5.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find demo_moveit)/config/joint_limits.yaml"/>
<rosparam command="load" file="$(find demo_moveit)/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 demo_moveit)/config/kinematics.yaml"/>
</group>
</launch>

10
mycobot_320m5_gazebo/demo_moveit/launch/planning_pipeline.launch.xml
View file

@ -1,10 +0,0 @@
<launch>
<!-- This file makes it easy to include different planning pipelines;
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
<include ns="$(arg pipeline)" file="$(dirname)/$(arg pipeline)_planning_pipeline.launch.xml" />
</launch>

4
mycobot_320m5_gazebo/demo_moveit/launch/ros_control_moveit_controller_manager.launch.xml
View file

@ -1,4 +0,0 @@
<launch>
<!-- Define MoveIt controller manager plugin -->
<param name="moveit_controller_manager" value="moveit_ros_control_interface::MoveItControllerManager" />
</launch>

11
mycobot_320m5_gazebo/demo_moveit/launch/ros_controllers.launch
View file

@ -1,11 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find demo_moveit)/config/ros_controllers.yaml" command="load"/>
<!-- Load the controllers -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" args="arm_controller gripper_controller "/>
</launch>

21
mycobot_320m5_gazebo/demo_moveit/launch/run_benchmark_ompl.launch
View file

@ -1,21 +0,0 @@
<launch>
<!-- This argument must specify the list of .cfg files to process for benchmarking -->
<arg name="cfg" />
<!-- Load URDF -->
<include file="$(dirname)/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- Start the database -->
<include file="$(dirname)/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
</include>
<!-- Start Benchmark Executable -->
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
<rosparam command="load" file="$(find demo_moveit)/config/ompl_planning.yaml"/>
</node>
</launch>

17
mycobot_320m5_gazebo/demo_moveit/launch/sensor_manager.launch.xml
View file

@ -1,17 +0,0 @@
<launch>
<!-- This file makes it easy to include the settings for sensor managers -->
<!-- Params for 3D sensors config -->
<rosparam command="load" file="$(find demo_moveit)/config/sensors_3d.yaml" />
<!-- Params for the octomap monitor -->
<!-- <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
<param name="octomap_resolution" type="double" value="0.025" />
<param name="max_range" type="double" value="5.0" />
<!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
<arg name="moveit_sensor_manager" default="ur5" />
<include file="$(dirname)/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

16
mycobot_320m5_gazebo/demo_moveit/launch/setup_assistant.launch
View file

@ -1,16 +0,0 @@
<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
<launch>
<!-- Debug Info -->
<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" />
<!-- Run -->
<node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
args="--config_pkg=demo_moveit"
launch-prefix="$(arg launch_prefix)"
required="true"
output="screen" />
</launch>

8
mycobot_320m5_gazebo/demo_moveit/launch/simple_moveit_controller_manager.launch.xml
View file

@ -1,8 +0,0 @@
<launch>
<!-- Define the MoveIt controller manager plugin to use for trajectory execution -->
<param name="moveit_controller_manager" value="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<!-- Load controller list to the parameter server -->
<rosparam file="$(find demo_moveit)/config/simple_moveit_controllers.yaml" />
<rosparam file="$(find demo_moveit)/config/ros_controllers.yaml" />
</launch>

23
mycobot_320m5_gazebo/demo_moveit/launch/stomp_planning_pipeline.launch.xml
View file

@ -1,23 +0,0 @@
<launch>
<!-- Stomp Plugin for MoveIt -->
<arg name="planning_plugin" value="stomp_moveit/StompPlannerManager" />
<arg name="start_state_max_bounds_error" value="0.1" />
<arg name="jiggle_fraction" value="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/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints" />
<param name="planning_plugin" value="$(arg planning_plugin)" />
<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 demo_moveit)/config/stomp_planning.yaml"/>
</launch>

23
mycobot_320m5_gazebo/demo_moveit/launch/trajectory_execution.launch.xml
View file

@ -1,23 +0,0 @@
<launch>
<!-- This file summarizes all settings required for trajectory execution -->
<!-- Define moveit controller manager plugin: fake, simple, or ros_control -->
<arg name="moveit_controller_manager" />
<arg name="fake_execution_type" default="interpolate" />
<!-- Flag indicating whether MoveIt is allowed to load/unload or switch controllers -->
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
<param name="trajectory_execution/allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Allowed joint-value tolerance for validation that trajectory's first point matches current robot state -->
<param name="trajectory_execution/allowed_start_tolerance" value="0.01"/> <!-- default 0.01 -->
<!-- We use pass_all_args=true here to pass fake_execution_type, which is required by fake controllers, but not by real-robot controllers.
As real-robot controller_manager.launch files shouldn't be required to define this argument, we use the trick of passing all args. -->
<include file="$(dirname)/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" pass_all_args="true" />
</launch>

3
mycobot_320m5_gazebo/demo_moveit/launch/ur5_moveit_sensor_manager.launch.xml
View file

@ -1,3 +0,0 @@
<launch>
</launch>

15
mycobot_320m5_gazebo/demo_moveit/launch/warehouse.launch
View file

@ -1,15 +0,0 @@
<launch>
<!-- The path to the database must be specified -->
<arg name="moveit_warehouse_database_path" />
<!-- Load warehouse parameters -->
<include file="$(dirname)/warehouse_settings.launch.xml" />
<!-- Run the DB server -->
<node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros_mongo">
<param name="overwrite" value="false"/>
<param name="database_path" value="$(arg moveit_warehouse_database_path)" />
</node>
</launch>

16
mycobot_320m5_gazebo/demo_moveit/launch/warehouse_settings.launch.xml
View file

@ -1,16 +0,0 @@
<launch>
<!-- Set the parameters for the warehouse and run the mongodb server. -->
<!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
<arg name="moveit_warehouse_port" default="33829" />
<!-- The default DB host for moveit -->
<arg name="moveit_warehouse_host" default="localhost" />
<!-- Set parameters for the warehouse -->
<param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
<param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
<param name="warehouse_exec" value="mongod" />
<param name="warehouse_plugin" value="warehouse_ros_mongo::MongoDatabaseConnection" />
</launch>

42
mycobot_320m5_gazebo/demo_moveit/package.xml
View file

@ -1,42 +0,0 @@
<package>
<name>demo_moveit</name>
<version>0.3.0</version>
<description>
An automatically generated package with all the configuration and launch files for using the ur5 with the MoveIt Motion Planning Framework
</description>
<author email="429229407@qq.com">lijie</author>
<maintainer email="429229407@qq.com">lijie</maintainer>
<license>BSD</license>
<url type="website">http://moveit.ros.org/</url>
<url type="bugtracker">https://github.com/moveit/moveit/issues</url>
<url type="repository">https://github.com/moveit/moveit</url>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_fake_controller_manager</run_depend>
<run_depend>moveit_kinematics</run_depend>
<run_depend>moveit_planners</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>moveit_setup_assistant</run_depend>
<run_depend>moveit_simple_controller_manager</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>joint_state_publisher_gui</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>rviz</run_depend>
<run_depend>tf2_ros</run_depend>
<run_depend>xacro</run_depend>
<!-- The next 2 packages are required for the gazebo simulation.
We don't include them by default to prevent installing gazebo and all its dependencies. -->
<!-- <run_depend>joint_trajectory_controller</run_depend> -->
<!-- <run_depend>gazebo_ros_control</run_depend> -->
<!-- This package is referenced in the warehouse launch files, but does not build out of the box at the moment. Commented the dependency until this works. -->
<!-- <run_depend>warehouse_ros_mongo</run_depend> -->
<run_depend>mycobot_320_description</run_depend>
</package>

25
mycobot_320m5_gazebo/gazebo-pkgs/Dockerfile
View file

@ -1,25 +0,0 @@
FROM jenniferbuehler/general-message-pkgs
MAINTAINER Jennifer Buehler
# Install required ROS dependencies
RUN apt-get update && apt-get install -y \
ros-indigo-gazebo-ros \
ros-indigo-eigen-conversions \
ros-indigo-roslint \
&& rm -rf /var/lib/apt/lists/
COPY gazebo_grasp_plugin /catkin_ws/src/gazebo_grasp_plugin
COPY gazebo_state_plugins /catkin_ws/src/gazebo_state_plugins
COPY gazebo_test_tools /catkin_ws/src/gazebo_test_tools
COPY gazebo_world_plugin_loader /catkin_ws/src/gazebo_world_plugin_loader
# Build
RUN bin/bash -c "source /.bashrc \
&& cd /catkin_ws \
&& catkin_make \
&& catkin_make install"
RUN bin/bash -c "source .bashrc"
CMD ["bash","-l"]

29
mycobot_320m5_gazebo/gazebo-pkgs/LICENSE
View file

@ -1,29 +0,0 @@
BSD 3-Clause License
Copyright (c) 2019, Jennifer Buehler
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
* Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

5
mycobot_320m5_gazebo/gazebo-pkgs/README.md
View file

@ -1,5 +0,0 @@
# gazebo-pkgs
A collection of tools and plugins for Gazebo.
Please also refer to [the wiki](https://github.com/JenniferBuehler/gazebo-pkgs/wiki) for more information.

13
mycobot_320m5_gazebo/gazebo-pkgs/TODO.md
View file

@ -1,13 +0,0 @@
- Include the local implementation of joint controller and joint trajectory execution (gazbo implementation) in these packages.
# Possible future contributions
gazebo:
- the higher-level controller with position and velocity
- a program which helps find PID values, either with neural network or Ziegler-Nichols method
# Objects
- move away from object_msgs and replace with object_recognition_msgs/RecognizedObject.msg. The bounding_mesh
can be used to replace primitives, which then have to be supported in object_moveit/ObjectMessageGenerator.

19
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/CHANGELOG.rst
View file

@ -1,19 +0,0 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package gazebo_grasp_plugin
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.2 (2018-01-06)
------------------
* Merge branch jacknlliu-fix-c11-error
* fix build error with c++ 11
* Contributors: Jack Liu, Jennifer Buehler
1.0.1 (2016-06-08)
------------------
* Fixed cmake files for jenkins builds
* Contributors: Jennifer Buehler
1.0.0 (2016-06-07)
------------------
* Initial release
* Contributors: Jennifer Buehler

144
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/CMakeLists.txt
View file

@ -1,144 +0,0 @@
cmake_minimum_required(VERSION 2.8.3)
project(gazebo_grasp_plugin)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
gazebo_ros
geometry_msgs
roscpp
std_msgs
gazebo_version_helpers
)
find_package(gazebo REQUIRED)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# Binary directory required for proto headers inclusion to work, because install commands don't
# get executed in devel space. The directory above is required so that an include of
# <gazebo_grasp_plugin/msgs/grasp_event.pb.h>
# also works in devel space like it needs to be in install space.
# Probably we can find a better solution for this, but until then this
# fix will be OK.
INCLUDE_DIRS include ${CMAKE_CURRENT_BINARY_DIR}/..
LIBRARIES gazebo_grasp_fix gazebo_grasp_msgs
CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs gazebo_version_helpers
DEPENDS gazebo
)
###########
## Build ##
###########
# check c++11 / c++0x
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "-std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "-std=c++0x")
else()
message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler. Suggested solution: update the pkg build-essential ")
endif()
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
include
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
)
add_subdirectory(msgs)
link_directories(
${catkin_LIBRARY_DIRS}
${GAZEBO_LIBRARY_DIRS}
${CMAKE_CURRENT_BINARY_DIR}/msgs
)
## Declare a cpp library
add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp src/GazeboGraspGripper.cpp)
## Add cmake target dependencies of the executable/library
## as an example, message headers may need to be generated before nodes
# add_dependencies(gazebo_grasp_plugin_node gazebo_grasp_plugin_generate_messages_cpp)
add_dependencies(gazebo_grasp_fix ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(gazebo_grasp_fix
gazebo_grasp_msgs
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
${Boost_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS gazebo_grasp_fix
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
FILES_MATCHING PATTERN "*.launch"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_gazebo_grasp_plugin.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

261
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h
View file

@ -1,261 +0,0 @@
#ifndef GAZEBO_GAZEBOGRASPFIX_H
#define GAZEBO_GAZEBOGRASPFIX_H
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
namespace gazebo
{
/**
* Inspired by gazebo::physics::Gripper, this plugin fixes an object which is grasped to the
* robot hand to avoid problems with physics engines and to help the object staying in
* the robot hand without slipping out.
*
* This is a *model* plugin, so you have to load the model plugin from the robot URDF:
*
* ```xml
* <gazebo>
* <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
* <arm>
* <arm_name>name-of-arm</arm_name>
* <palm_link> hand_link_name </palm_link>
* <gripper_link> finger_index_link_1 </gripper_link>
* <gripper_link> finger_index_link_2 </gripper_link>
* <gripper_link> ... </gripper_link>
* </arm>
* <forces_angle_tolerance>100</forces_angle_tolerance>
* <update_rate>4</update_rate>
* <grip_count_threshold>4</grip_count_threshold>
* <max_grip_count>8</max_grip_count>
* <release_tolerance>0.005</release_tolerance>
* <disable_collisions_on_attach>false</disable_collisions_on_attach>
* <contact_topic>__default_topic__</contact_topic>
* </plugin>
* </gazebo>
* ```
*
* Description of the arguments:
*
* - ``<arm>`` contains a collections of specification for each arm which can grasp one object (there may be several ``<arm>`` tags):
* - ``<arm_name>`` is the name of this arm. Has to be unique.
* - ``<palm_link>`` has to be the link to which the finger joints are attached.
* - ``<gripper_link>`` tags have to include -all- link names of the gripper/hand which are used to
* actively grasp objects (these are the links which determine whether a "grasp" exists according to
* above described criterion).
* - ``<update_rate>`` is the rate at which all contact points are checked against the "gripping criterion".
* Note that in-between such updates, existing contact points may be collected at
* a higher rate (the Gazebo world update rate). The ``update_rate`` is only the rate at
* which they are processed, which takes a bit of computation time, and therefore
* should be lower than the gazebo world update rate.
* - ``<forces_angle_tolerance>`` is the tolerance angle (in degrees) between two force vectors to be considered
* "opposing forces". If the angle is smaller than this, they are not opposing.
* - ``<grip_count_threshold>`` is number of times in the update loop (running at update_rate) that an object has
* to be detected as "gripped" in order to attach the object.
* Adjust this with the update rate.
* - ``<max_grip_count>`` is the maximum number of a counter:
* At each update iteration (running at update_rate), if the "gripping criterion" is
* met for an object, a counter for this object is increased. ``max_grip_count`` is
* the maximum number recorded for an object. As soon as the counter goes beyond this
* number, the counter is stopped. As soon as the "gripping criterion" does not
* hold any more, the number will start to decrease again, (by 1 each time the object
* is detected as "not grasped" in an update iteration). So this counter is
* like a "buffer" which, when it is full, maintains the state, and when it is empty,
* again, the object is released.
* This should be at least double of ``grip_count_threshold``.
* - ``<release_tolerance>`` is the distance which the gripper links are allowed to move away from the object
* during- a grasp without the object being detached, even if there are currently no
* actual contacts on the object. This condition can happen if the fingers "wobble"
* or move ever so slightly away from the object, and therefore the "gripping criterion"
* fails in a few subsequent update iterations. This setting is to make the behaviour more
* stable.
* Setting this number too high will also lead to the object not being detached even
* if the grippers have opened up to release it, so use this with care.
* - ``<disable_collisions_on_attach>`` can be used for the following:
* When an object is attached, collisions with it may be disabled, in case the
* robot still keeps wobbling.
* - ``<contact_topic>`` is the gazebo topic of contacts. Should normally be left at -\_\_default_topic\_\_-.
*
* Current limitations:
*
* - Only one object can be attached per gripper.
* - Only partial support for an object cannot be gripped with two grippers (release condition may be
* triggered wrongly, or not at all, if two grippers are involved)
*
* \author Jennifer Buehler
*/
class GazeboGraspFix : public ModelPlugin
{
public:
GazeboGraspFix();
GazeboGraspFix(physics::ModelPtr _model);
virtual ~GazeboGraspFix();
private:
/**
* Gets called just after the object has been attached to the palm link on \e armName
*/
void OnAttach(const std::string &objectName,
const std::string &armName);
/**
* Gets called just after the object has been detached to the palm link on \e armName
*/
void OnDetach(const std::string &objectName,
const std::string &armName);
virtual void Init();
virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
/**
* Collects for each object all forces which are currently applied on it.
* Then, for each object, checks whether of all the forces applied,
* there are opposing forces.
* This is done by calling CheckGrip() with the list of all forces applied.
* If CheckGrip() returns true, the number of "grip counts"
* is increased for the holding arm (but grip counts will never exceed \e max_grip_count).
* If the number of grip counts for this object exceeds \e grip_count_threshold,
* the object is attached by calling GazeboGraspGripper::HandleAttach(object-name),
* setting \e attached and \e attachedObjName, and \e GazeboGraspGripper::attachGripContacts
* is updated with the contact points currently existing for this object (current entry in \e contacts).
*
* Then, goes through all entries in \e gripCount, and unless it's an object
* we just detected as "gripped", the counter is decreased.
* If the counter is is smaller than \e grip_count_threshold, the object should
* potentially be released, but this criterion happens too easily
* (the fingers in gazebo may have started wobbling as the arm moves around, and although they are still
* close to the object, the grip is not detected any more).
* So to be sure, and additional criterion has to be satisfied before the object is released:
* check that the collision point (the place on the link where the contact originally
* was detected) has not moved too far from where it originally was, relative to the object.
*/
void OnUpdate();
void InitValues();
/**
* Gets called upon detection of contacts.
* A list of contacts is passed in \_msg. One contact has two bodies, and only
* the ones where one of the bodies is a gripper link are considered.
* Each contact consists of a *list* of forces with their own origin/position each
* (e.g. when the object and gripper are colliding at several places).
* The averages of each contact's force vectors along with their origins is computed.
* This "average contact force/origin" for each contact is then added to the \e this->contacts map.
* If an entry for this object/link pair already exists, the average force (and its origin)
* is *added* to the existing force/origin, and the average count is increased. This is to get
* the average force application over time between link and object.
*/
void OnContact(const ConstContactsPtr &ptr);
// bool CheckGrip(const std::vector<GzVector3> &forces, float minAngleDiff,
// float lengthRatio);
bool IsGripperLink(const std::string &linkName, std::string &gripperName) const;
/**
* return objects (key) and the gripper (value) to which it is attached
*/
std::map<std::string, std::string> GetAttachedObjects() const;
/**
* Helper class to collect contact info per object.
* Forward declaration here.
*/
class ObjectContactInfo;
/**
* Helper function to determine if object attached to a gripper in ObjectContactInfo.
*/
bool ObjectAttachedToGripper(const ObjectContactInfo &objContInfo,
std::string &attachedToGripper) const;
/**
* Helper function to determine if object attached to this gripper
*/
bool ObjectAttachedToGripper(const std::string &gripperName,
std::string &attachedToGripper) const;
// physics::ModelPtr model;
// physics::PhysicsEnginePtr physics;
physics::WorldPtr world;
// sorted by their name, all grippers of the robot
std::map<std::string, GazeboGraspGripper> grippers;
event::ConnectionPtr update_connection;
transport::NodePtr node;
transport::PublisherPtr eventsPub; // publisher of grasping events
transport::SubscriberPtr contactSub; // subscriber to contact updates
// tolerance (in degrees) between force vectors to
// be considered "opposing"
float forcesAngleTolerance;
// when an object is attached, collisions with it may be disabled, in case the
// robot still keeps wobbling.
bool disableCollisionsOnAttach;
// all collisions per gazebo collision link (each entry
// belongs to a physics::CollisionPtr element). The key
// is the collision link name, the value is the gripper name
// this collision link belongs to.
std::map<std::string, std::string> collisions;
/**
* Helper class to encapsulate a collision information. Forward declaration here.
*/
class CollidingPoint;
// Contact forces sorted by object name the gripper collides with (first key)
// and the link colliding (second key).
std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
boost::mutex mutexContacts; //mutex protects contacts
// when an object was first attached, it had these colliding points.
// First key is object name, second is the link colliding, as in \e contacts.
// Only the links of *one* gripper are stored here. This indirectly imposes the
// limitation that no two grippers can grasp the object (while it would be
// possible, the release condition is tied to only one link, so the object may
// not be released properly).
std::map<std::string, std::map<std::string, CollidingPoint> >
attachGripContacts;
// Records how many subsequent update calls the grip on that object has been recorded
// as "holding". Every loop, if a grip is not recorded, this number decreases.
// When it reaches \e grip_count_threshold, it will be attached.
// The number won't increase above max_grip_count once it has reached that number.
std::map<std::string, int> gripCounts;
// *maximum* number in \e gripCounts to be recorded.
int maxGripCount;
// number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts
// as the object grasped, and when it is lower, as released.
int gripCountThreshold;
// once an object is gripped, the relative position of the collision link surface to the
// object is remembered. As soon as this distance changes more than release_tolerance,
// the object is released.
float releaseTolerance;
//nano seconds between two updates
common::Time updateRate;
//last time OnUpdate() was called
common::Time prevUpdateTime;
//ContactManager filter to be removed in destructor
std::string filter_name;
};
}
#endif // GAZEBO_GAZEBOGRASPFIX_H

89
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspGripper.h
View file

@ -1,89 +0,0 @@
#ifndef GAZEBO_GAZEBOGRASPGRIPPER_H
#define GAZEBO_GAZEBOGRASPGRIPPER_H
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
namespace gazebo
{
/**
* \brief Helper class for GazeboGraspFix which holds information for one arm.
* Attaches /detaches objects to the palm of this arm.
*
* \author Jennifer Buehler
*/
class GazeboGraspGripper
{
public:
GazeboGraspGripper();
GazeboGraspGripper(const GazeboGraspGripper &o);
virtual ~GazeboGraspGripper();
/**
*
* \param disableCollisionsOnAttach when an object is attached, collisions with it will be disabled. This is useful
* if the robot then still keeps wobbling.
*/
bool Init(physics::ModelPtr &_model,
const std::string &_gripperName,
const std::string &palmLinkName,
const std::vector<std::string> &fingerLinkNames,
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr> &_collisions);
const std::string &getGripperName() const;
/**
* Has the link name (URDF)
*/
bool hasLink(const std::string &linkName) const;
/**
* Has the collision link name (Gazebo collision element name)
*/
bool hasCollisionLink(const std::string &linkName) const;
bool isObjectAttached() const;
const std::string &attachedObject() const;
/**
* \param gripContacts contact forces on the object sorted by the link name colliding.
*/
bool HandleAttach(const std::string &objName);
void HandleDetach(const std::string &objName);
private:
physics::ModelPtr model;
// name of the gripper
std::string gripperName;
// names of the gripper links
std::vector<std::string> linkNames;
// names and Collision objects of the collision links in Gazebo (scoped names)
// Not necessarily equal names and size to linkNames.
std::map<std::string, physics::CollisionPtr> collisionElems;
physics::JointPtr fixedJoint;
physics::LinkPtr palmLink;
// when an object is attached, collisions with it may be disabled, in case the
// robot still keeps wobbling.
bool disableCollisionsOnAttach;
// flag holding whether an object is attached. Object name in \e attachedObjName
bool attached;
// name of the object currently attached.
std::string attachedObjName;
};
}
#endif // GAZEBO_GAZEBOGRASPGRIPPER_H

20
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/msgs/CMakeLists.txt
View file

@ -1,20 +0,0 @@
find_package(Protobuf REQUIRED)
set(msgs
grasp_event.proto
)
PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs})
add_library(gazebo_grasp_msgs SHARED ${PROTO_SRCS})
target_link_libraries(gazebo_grasp_msgs ${PROTOBUF_LIBRARY})
## Mark executables and/or libraries for installation
install(TARGETS gazebo_grasp_msgs
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.pb.h"
)

17
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/msgs/grasp_event.proto
View file

@ -1,17 +0,0 @@
syntax = "proto2";
package gazebo.msgs;
/// \ingroup gazebo_msgs
/// \interface GraspEvent
/// \brief Message to notify about grasp events
message GraspEvent
{
// name of grasping arm/gripper
required string arm = 1;
// collision shape name of grasped object
required string object = 2;
// detached if false
required bool attached = 3;
}

51
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/package.xml
View file

@ -1,51 +0,0 @@
<?xml version="1.0"?>
<package>
<name>gazebo_grasp_plugin</name>
<version>1.0.2</version>
<description>
Gazebo Model plugin(s) which handle/help grasping in Gazebo.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="jennifer.e.buehler@gmail.com">Jennifer Buehler</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>GPLv3</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/gazebo_grasp_plugin</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="jennifer.e.buehler@gmail.com">Jennifer Buehler</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>gazebo_ros</build_depend>
<build_depend>gazebo</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>gazebo_version_helpers</build_depend>
<run_depend>gazebo_ros</run_depend>
<run_depend>gazebo</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>gazebo_version_helpers</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

981
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/src/GazeboGraspFix.cpp
View file

@ -1,981 +0,0 @@
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/ContactManager.hh>
#include <gazebo/physics/Contact.hh>
#include <gazebo/common/common.hh>
#include <gazebo_grasp_plugin/GazeboGraspFix.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
#include <msgs/grasp_event.pb.h>
using gazebo::GazeboGraspFix;
using gazebo::GzVector3;
#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
#define DEFAULT_UPDATE_RATE 5
#define DEFAULT_MAX_GRIP_COUNT 10
#define DEFAULT_RELEASE_TOLERANCE 0.005
#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
// Register this plugin with the simulator
GZ_REGISTER_MODEL_PLUGIN(GazeboGraspFix)
////////////////////////////////////////////////////////////////////////////////
GazeboGraspFix::GazeboGraspFix()
{
InitValues();
}
////////////////////////////////////////////////////////////////////////////////
GazeboGraspFix::GazeboGraspFix(physics::ModelPtr _model)
{
InitValues();
}
////////////////////////////////////////////////////////////////////////////////
GazeboGraspFix::~GazeboGraspFix()
{
// Release filter to make it safe to reload the model with plugin
if (!filter_name.empty() && this->world)
{
physics::PhysicsEnginePtr physics = GetPhysics(this->world);
physics::ContactManager *contactManager = physics->GetContactManager();
if (contactManager)
contactManager->RemoveFilter(filter_name);
}
this->update_connection.reset();
if (this->node) this->node->Fini();
this->node.reset();
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::Init()
{
this->prevUpdateTime = common::Time::GetWallTime();
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::InitValues()
{
#if GAZEBO_MAJOR_VERSION > 2
gazebo::common::Console::SetQuiet(false);
#endif
// float timeDiff=0.25;
// this->releaseTolerance=0.005;
// this->updateRate = common::Time(0, common::Time::SecToNano(timeDiff));
this->prevUpdateTime = common::Time::GetWallTime();
//float graspedSecs=2;
//this->maxGripCount=floor(graspedSecs/timeDiff);
//this->gripCountThreshold=floor(this->maxGripCount/2);
this->node = transport::NodePtr(new transport::Node());
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
gzmsg << "Loading grasp-fix plugin" << std::endl;
// ++++++++++++ Read parameters and initialize fields +++++++++++++++
physics::ModelPtr model = _parent;
this->world = model->GetWorld();
sdf::ElementPtr disableCollisionsOnAttachElem =
_sdf->GetElement("disable_collisions_on_attach");
if (!disableCollisionsOnAttachElem)
{
gzmsg << "GazeboGraspFix: Using default " <<
DEFAULT_DISABLE_COLLISIONS_ON_ATTACH <<
" because no <disable_collisions_on_attach> element specified." <<
std::endl;
this->disableCollisionsOnAttach = DEFAULT_DISABLE_COLLISIONS_ON_ATTACH;
}
else
{
std::string str = disableCollisionsOnAttachElem->Get<std::string>();
bool bVal = false;
if ((str == "true") || (str == "1")) bVal = true;
this->disableCollisionsOnAttach = bVal;
gzmsg << "GazeboGraspFix: Using disable_collisions_on_attach " <<
this->disableCollisionsOnAttach << std::endl;
}
sdf::ElementPtr forcesAngleToleranceElem =
_sdf->GetElement("forces_angle_tolerance");
if (!forcesAngleToleranceElem)
{
gzmsg << "GazeboGraspFix: Using default tolerance of " <<
DEFAULT_FORCES_ANGLE_TOLERANCE <<
" because no <forces_angle_tolerance> element specified." <<
std::endl;
this->forcesAngleTolerance = DEFAULT_FORCES_ANGLE_TOLERANCE * M_PI / 180;
}
else
{
this->forcesAngleTolerance =
forcesAngleToleranceElem->Get<float>() * M_PI / 180;
}
sdf::ElementPtr updateRateElem = _sdf->GetElement("update_rate");
double _updateSecs;
if (!updateRateElem)
{
gzmsg << "GazeboGraspFix: Using " << DEFAULT_UPDATE_RATE <<
" because no <updateRate_tag> element specified." << std::endl;
_updateSecs = 1.0 / DEFAULT_UPDATE_RATE;
}
else
{
int _rate = updateRateElem->Get<int>();
double _updateRate = _rate;
_updateSecs = 1.0 / _updateRate;
gzmsg << "GazeboGraspFix: Using update rate " << _rate << std::endl;
}
this->updateRate = common::Time(0, common::Time::SecToNano(_updateSecs));
sdf::ElementPtr maxGripCountElem = _sdf->GetElement("max_grip_count");
if (!maxGripCountElem)
{
gzmsg << "GazeboGraspFix: Using " << DEFAULT_MAX_GRIP_COUNT <<
" because no <max_grip_count> element specified." << std::endl;
this->maxGripCount = DEFAULT_MAX_GRIP_COUNT;
}
else
{
this->maxGripCount = maxGripCountElem->Get<int>();
gzmsg << "GazeboGraspFix: Using max_grip_count "
<< this->maxGripCount << std::endl;
}
sdf::ElementPtr gripCountThresholdElem =
_sdf->GetElement("grip_count_threshold");
if (!gripCountThresholdElem)
{
this->gripCountThreshold = floor(this->maxGripCount / 2.0);
gzmsg << "GazeboGraspFix: Using " << this->gripCountThreshold <<
" because no <grip_count_threshold> element specified." <<
std::endl;
}
else
{
this->gripCountThreshold = gripCountThresholdElem->Get<int>();
gzmsg << "GazeboGraspFix: Using grip_count_threshold " <<
this->gripCountThreshold << std::endl;
}
sdf::ElementPtr releaseToleranceElem = _sdf->GetElement("release_tolerance");
if (!releaseToleranceElem)
{
gzmsg << "GazeboGraspFix: Using " << DEFAULT_RELEASE_TOLERANCE <<
" because no <release_tolerance> element specified." << std::endl;
this->releaseTolerance = DEFAULT_RELEASE_TOLERANCE;
}
else
{
this->releaseTolerance = releaseToleranceElem->Get<float>();
gzmsg << "GazeboGraspFix: Using release_tolerance " <<
this->releaseTolerance << std::endl;
}
// will contain all names of collision entities involved from all arms
std::vector<std::string> collisionNames;
sdf::ElementPtr armElem = _sdf->GetElement("arm");
if (!armElem)
{
gzerr << "GazeboGraspFix: Cannot load the GazeboGraspFix without any <arm> declarations"
<< std::endl;
return;
}
// add all arms:
for (; armElem != NULL; armElem = armElem->GetNextElement("arm"))
{
sdf::ElementPtr armNameElem = armElem->GetElement("arm_name");
sdf::ElementPtr handLinkElem = armElem->GetElement("palm_link");
sdf::ElementPtr fingerLinkElem = armElem->GetElement("gripper_link");
if (!handLinkElem || !fingerLinkElem || !armNameElem)
{
gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix arm because "
<< "not all of <arm_name>, <palm_link> and <gripper_link> elements specified in URDF/SDF. Skipping."
<< std::endl;
continue;
}
std::string armName = armNameElem->Get<std::string>();
std::string palmName = handLinkElem->Get<std::string>();
// collect all finger names:
std::vector<std::string> fingerLinkNames;
for (; fingerLinkElem != NULL;
fingerLinkElem = fingerLinkElem->GetNextElement("gripper_link"))
{
std::string linkName = fingerLinkElem->Get<std::string>();
fingerLinkNames.push_back(linkName);
}
// add new gripper
if (grippers.find(armName) != grippers.end())
{
gzerr << "GazeboGraspFix: Arm named " << armName <<
" was already added, cannot add it twice." << std::endl;
}
GazeboGraspGripper &gripper = grippers[armName];
std::map<std::string, physics::CollisionPtr> _collisions;
if (!gripper.Init(_parent, armName, palmName, fingerLinkNames,
disableCollisionsOnAttach, _collisions))
{
gzerr << "GazeboGraspFix: Could not initialize arm " << armName << ". Skipping."
<< std::endl;
grippers.erase(armName);
continue;
}
// add all the grippers's collision elements
for (std::map<std::string, physics::CollisionPtr>::iterator collIt =
_collisions.begin();
collIt != _collisions.end(); ++collIt)
{
const std::string &collName = collIt->first;
//physics::CollisionPtr& coll=collIt->second;
std::map<std::string, std::string>::iterator collIter = this->collisions.find(
collName);
if (collIter !=
this->collisions.end()) //this collision was already added before
{
gzwarn << "GazeboGraspFix: Adding Gazebo collision link element " << collName <<
" multiple times, the grasp plugin may not work properly" << std::endl;
continue;
}
gzmsg << "GazeboGraspFix: Adding collision scoped name " << collName <<
std::endl;
this->collisions[collName] = armName;
collisionNames.push_back(collName);
}
}
if (grippers.empty())
{
gzerr << "ERROR: GazeboGraspFix: Cannot use a GazeboGraspFix because "
<< "no arms were configured successfully. Plugin will not work." << std::endl;
return;
}
// ++++++++++++ start up things +++++++++++++++
physics::PhysicsEnginePtr physics = GetPhysics(this->world);
this->node->Init(gazebo::GetName(*(this->world)));
physics::ContactManager *contactManager = physics->GetContactManager();
contactManager->PublishContacts(); // TODO not sure this is required?
filter_name = model->GetScopedName();
std::string topic = contactManager->CreateFilter(filter_name,
collisionNames);
if (!this->contactSub)
{
gzmsg << "Subscribing contact manager to topic " << topic << std::endl;
bool latching = false;
this->contactSub = this->node->Subscribe(topic, &GazeboGraspFix::OnContact,
this, latching);
}
gzmsg << "Advertising grasping events on topic grasp_events" << std::endl;
this->eventsPub = this->node->Advertise<msgs::GraspEvent>("~/grasp_events");
update_connection = event::Events::ConnectWorldUpdateEnd(boost::bind(
&GazeboGraspFix::OnUpdate, this));
}
////////////////////////////////////////////////////////////////////////////////
class GazeboGraspFix::ObjectContactInfo
{
public:
// all forces effecting on the object
std::vector<GzVector3> appliedForces;
// all grippers involved in the process, along with
// a number counting the number of contact points with the
// object per gripper
std::map<std::string, int> grippersInvolved;
// maximum number of contacts of *any one* gripper
// (any in grippersInvolved)
int maxGripperContactCnt;
// the gripper for maxGripperContactCnt
std::string maxContactGripper;
};
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::IsGripperLink(const std::string &linkName,
std::string &gripperName) const
{
for (std::map<std::string, GazeboGraspGripper>::const_iterator it =
grippers.begin(); it != grippers.end(); ++it)
{
if (it->second.hasLink(linkName))
{
gripperName = it->first;
return true;
}
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
std::map<std::string, std::string> GazeboGraspFix::GetAttachedObjects() const
{
std::map<std::string, std::string> ret;
for (std::map<std::string, GazeboGraspGripper>::const_iterator it =
grippers.begin(); it != grippers.end(); ++it)
{
const std::string &gripperName = it->first;
const GazeboGraspGripper &gripper = it->second;
if (gripper.isObjectAttached())
{
ret[gripper.attachedObject()] = gripperName;
}
}
return ret;
}
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const ObjectContactInfo
&objContInfo, std::string &attachedToGripper) const
{
for (std::map<std::string, int>::const_iterator gripInvIt =
objContInfo.grippersInvolved.begin();
gripInvIt != objContInfo.grippersInvolved.end(); ++gripInvIt)
{
const std::string &gripperName = gripInvIt->first;
if (ObjectAttachedToGripper(gripperName, attachedToGripper))
{
return true;
}
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
bool GazeboGraspFix::ObjectAttachedToGripper(const std::string &gripperName,
std::string &attachedToGripper) const
{
std::map<std::string, GazeboGraspGripper>::const_iterator gIt = grippers.find(
gripperName);
if (gIt == grippers.end())
{
gzerr << "GazeboGraspFix: Inconsistency, gripper " << gripperName <<
" not found in GazeboGraspFix grippers" << std::endl;
return false;
}
const GazeboGraspGripper &gripper = gIt->second;
// gzmsg<<"Gripper "<<gripperName<<" is involved in the grasp"<<std::endl;
if (gripper.isObjectAttached())
{
attachedToGripper = gripperName;
return true;
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
/**
* Helper class to encapsulate a collision information.
* One contact has two bodies, and only
* the ones where one of the bodies is a gripper link are considered.
* Each contact consists of a *list* of forces with their own origin/position each
* (e.g. when the object and gripper are colliding at several places).
* The averages of each contact's force vectors along with their origins are
* *accumulated* in the given Vector3 \e pos and \eforce objects.
* The number of additions is stored in \e sum.
* This is to get the average force application over time between link and object.
*
* \author Jennifer Buehler
*/
class GazeboGraspFix::CollidingPoint
{
public:
CollidingPoint(): sum(0) {}
CollidingPoint(const CollidingPoint &o):
gripperName(o.gripperName),
collLink(o.collLink),
collObj(o.collObj),
force(o.force),
pos(o.pos),
objPos(o.objPos),
sum(o.sum) {}
// Name of the gripper/arm involved in contact point
// This is not the specific link, but the name of the
// whole gripper
std::string gripperName;
// the collision
physics::CollisionPtr collLink, collObj;
// average force vector of the colliding point
GzVector3 force;
// position (relative to reference frame of gripper
// collision surface) where the contact happens on collision surface
GzVector3 pos;
// position (relative to reference frame of *gripper* collision surface)
// where the object center is located during collision.
GzVector3 objPos;
// sum of force and pose (they are actually summed
// up from several contact points).
// Divide both \e force and \e pos by this to obtain average
int sum;
};
////////////////////////////////////////////////////////////////////////////////
double AngularDistance(const GzVector3 &_v1,
const GzVector3 &_v2)
{
GzVector3 v1 = _v1;
GzVector3 v2 = _v2;
v1.Normalize();
v2.Normalize();
return acos(v1.Dot(v2));
}
////////////////////////////////////////////////////////////////////////////////
// Checks whether any two vectors in the set have an angle greater
// than minAngleDiff (in rad), and one is at least
// lengthRatio (0..1) of the other in it's length.
bool CheckGrip(const std::vector<GzVector3> &forces,
float minAngleDiff, float lengthRatio)
{
if (((lengthRatio > 1) || (lengthRatio < 0)) && (lengthRatio > 1e-04
&& (fabs(lengthRatio - 1) > 1e-04)))
{
std::cerr << "ERROR: CheckGrip: always specify a lengthRatio of [0..1]" <<
std::endl;
return false;
}
if (minAngleDiff < M_PI_2)
{
std::cerr << "ERROR: CheckGrip: min angle must be at least 90 degrees (PI/2)" <<
std::endl;
return false;
}
std::vector<GzVector3>::const_iterator it1, it2;
for (it1 = forces.begin(); it1 != forces.end(); ++it1)
{
GzVector3 v1 = *it1;
for (it2 = it1 + 1; it2 != forces.end(); ++it2)
{
GzVector3 v2 = *it2;
float l1 = gazebo::GetLength(v1);
float l2 = gazebo::GetLength(v2);
if ((l1 < 1e-04) || (l2 < 1e-04)) continue;
/*GzVector3 _v1=v1;
GzVector3 _v2=v2;
_v1/=l1;
_v2/=l2;
float angle=acos(_v1.Dot(_v2));*/
float angle = AngularDistance(v1, v2);
// gzmsg<<"Angular distance between v1.len="<<v1.GetLength()<<" and v2.len="<<v2.GetLength()<<": "<<angle*180/M_PI<<std::endl;
if (angle > minAngleDiff)
{
float ratio;
if (l1 > l2) ratio = l2 / l1;
else ratio = l1 / l2;
// gzmsg<<"Got angle "<<angle<<", ratio "<<ratio<<std::endl;
if (ratio >= lengthRatio)
{
// gzmsg<<"CheckGrip() is true"<<std::endl;
return true;
}
}
}
}
return false;
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnUpdate()
{
if ((common::Time::GetWallTime() - this->prevUpdateTime) < this->updateRate)
return;
// first, copy all contact data into local struct. Don't do the complex grip check (CheckGrip)
// within the mutex, because that slows down OnContact().
this->mutexContacts.lock();
std::map<std::string, std::map<std::string, CollidingPoint> > contPoints(
this->contacts);
this->contacts.clear(); // clear so it can be filled anew by OnContact().
this->mutexContacts.unlock();
// contPoints now contains CollidingPoint objects for each *object* and *link*.
// Iterate through all contact points to gather all summed forces
// (and other useful information) for all the objects (so we have all forces on one object).
std::map<std::string, std::map<std::string, CollidingPoint> >::iterator objIt;
std::map<std::string, ObjectContactInfo> objectContactInfo;
for (objIt = contPoints.begin(); objIt != contPoints.end(); ++objIt)
{
std::string objName = objIt->first;
//gzmsg<<"Examining object collisions with "<<objName<<std::endl;
// create new entry in accumulated results map and get reference to fill in:
ObjectContactInfo &objContInfo = objectContactInfo[objName];
// for all links colliding with this object...
std::map<std::string, CollidingPoint>::iterator lIt;
for (lIt = objIt->second.begin(); lIt != objIt->second.end(); ++lIt)
{
std::string linkName = lIt->first;
CollidingPoint &collP = lIt->second;
GzVector3 avgForce = collP.force / collP.sum;
// gzmsg << "Found collision with "<<linkName<<": "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" (avg over "<<collP.sum<<")"<<std::endl;
objContInfo.appliedForces.push_back(avgForce);
// insert the gripper (if it doesn't exist yet) and increase contact counter
int &gContactCnt = objContInfo.grippersInvolved[collP.gripperName];
gContactCnt++;
int &_maxGripperContactCnt = objContInfo.maxGripperContactCnt;
if (gContactCnt > _maxGripperContactCnt)
{
_maxGripperContactCnt = gContactCnt;
objContInfo.maxContactGripper = collP.gripperName;
}
}
}
// ++++++++++++++++++++ Handle Attachment +++++++++++++++++++++++++++++++
// collect of all objects which are found to be "gripped" at the current stage.
// if they are gripped, increase the grip counter. If the grip count exceeds the
// threshold, attach the object to the gripper which has most contact points with the
// object.
std::set<std::string> grippedObjects;
for (std::map<std::string, ObjectContactInfo>::iterator ocIt =
objectContactInfo.begin();
ocIt != objectContactInfo.end(); ++ocIt)
{
const std::string &objName = ocIt->first;
const ObjectContactInfo &objContInfo = ocIt->second;
// gzmsg<<"Number applied forces on "<<objName<<": "<<objContInfo.appliedForces.size()<<std::endl;
// TODO: remove this test print, for issue #26 -------------------
#if 0
physics::CollisionPtr objColl =
boost::dynamic_pointer_cast<physics::Collision>(GetEntityByName(world, objName));
if (objColl && objColl->GetLink())
{
auto linVel = GetWorldVelocity(objColl->GetLink());
gzmsg << "Velocity for link " << objColl->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
float minAngleDiff = this->forcesAngleTolerance; //120 * M_PI/180;
if (!CheckGrip(objContInfo.appliedForces, minAngleDiff, 0.3))
continue;
// add to "gripped objects"
grippedObjects.insert(objName);
//gzmsg<<"Grasp Held: "<<objName<<" grip count: "<<this->gripCounts[objName]<<std::endl;
int &counts = this->gripCounts[objName];
if (counts < this->maxGripCount) ++counts;
// only need to attach object if the grip count threshold is exceeded
if (counts <= this->gripCountThreshold)
continue;
//gzmsg<<"GRIPPING "<<objName<<", grip count "<<counts<<" (threshold "<<this->gripCountThreshold<<")"<<std::endl;
// find out if any of the grippers involved in the grasp is already grasping the object.
// If there is no such gripper, attach it to the gripper which has most contact points.
std::string attachedToGripper;
bool isAttachedToGripper = ObjectAttachedToGripper(objContInfo,
attachedToGripper);
if (isAttachedToGripper)
{
// the object is already attached to a gripper, so it does not need to be attached.
// gzmsg << "GazeboGraspFix has found that object "<<
// gripper.attachedObject()<<" is already attached to gripper "<<gripperName;
continue;
}
// attach the object to the gripper with most contact counts
const std::string &graspingGripperName = objContInfo.maxContactGripper;
std::map<std::string, GazeboGraspGripper>::iterator gIt = grippers.find(
graspingGripperName);
if (gIt == grippers.end())
{
gzerr << "GazeboGraspFix: Inconsistency, gripper '" << graspingGripperName
<< "' not found in GazeboGraspFix grippers, so cannot do attachment of object "
<< objName << std::endl;
continue;
}
GazeboGraspGripper &graspingGripper = gIt->second;
if (graspingGripper.isObjectAttached())
{
gzerr << "GazeboGraspFix has found that object " <<
graspingGripper.attachedObject() << " is already attached to gripper " <<
graspingGripperName << ", so can't grasp '" << objName << "'!" << std::endl;
continue;
}
gzmsg << "GazeboGraspFix: Attaching " << objName << " to gripper " <<
graspingGripperName << "." << std::endl;
// Store the array of contact poses which played part in the grip, sorted by colliding link.
// Filter out all link names of other grippers, otherwise if the other gripper moves
// away, this is going to trigger the release condition.
// XXX this does not consider full support for an object being gripped by two grippers (e.g.
// one left, one right).
// this->attachGripContacts[objName]=contPoints[objName];
const std::map<std::string, CollidingPoint> &contPointsTmp =
contPoints[objName];
std::map<std::string, CollidingPoint> &attGripConts =
this->attachGripContacts[objName];
attGripConts.clear();
std::map<std::string, CollidingPoint>::const_iterator contPointsIt;
for (contPointsIt = contPointsTmp.begin(); contPointsIt != contPointsTmp.end();
++contPointsIt)
{
const std::string &collidingLink = contPointsIt->first;
const CollidingPoint &collidingPoint = contPointsIt->second;
// gzmsg<<"Checking initial contact with "<<collidingLink<<" and "<<graspingGripperName<<std::endl;
if (graspingGripper.hasCollisionLink(collidingLink))
{
// gzmsg<<"Insert initial contact with "<<collidingLink<<std::endl;
attGripConts[collidingLink] = collidingPoint;
}
}
if (!graspingGripper.HandleAttach(objName))
{
gzerr << "GazeboGraspFix: Could not attach object " << objName << " to gripper "
<< graspingGripperName << std::endl;
}
this->OnAttach(objName, graspingGripperName);
} // for all objects
// ++++++++++++++++++++ Handle Detachment +++++++++++++++++++++++++++++++
std::map<std::string, std::string> attachedObjects = GetAttachedObjects();
// now, for all objects that are not currently gripped,
// decrease grip counter, and possibly release object.
std::map<std::string, int>::iterator gripCntIt;
for (gripCntIt = this->gripCounts.begin(); gripCntIt != this->gripCounts.end();
++gripCntIt)
{
const std::string &objName = gripCntIt->first;
if (grippedObjects.find(objName) != grippedObjects.end())
{
// this object is one we just detected as "gripped", so no need to check for releasing it...
// gzmsg<<"NOT considering "<<objName<<" for detachment."<<std::endl;
continue;
}
// the object does not satisfy "gripped" criteria, so potentially has to be released.
// gzmsg<<"NOT-GRIPPING "<<objName<<", grip count "<<gripCntIt->second<<" (threshold "<<this->gripCountThreshold<<")"<<std::endl;
if (gripCntIt->second > 0) --(gripCntIt->second);
std::map<std::string, std::string>::iterator attIt = attachedObjects.find(
objName);
bool isAttached = (attIt != attachedObjects.end());
// gzmsg<<"is attached: "<<isAttached<<std::endl;
if (!isAttached || (gripCntIt->second > this->gripCountThreshold)) continue;
const std::string &graspingGripperName = attIt->second;
// gzmsg<<"Considering "<<objName<<" for detachment."<<std::endl;
// Object should potentially be detached now.
// However, this happens too easily when just considering the count, as the fingers
// in gazebo start wobbling as the arm moves around, and although they are still
// close to the object, the grip is not detected any more. So to be sure, we will
// check that the collision point (the place on the link where the contact originally
// was detected) has not moved too far from where it originally was, relative to the object.
// get the initial set of CollidingPoints for this object
// note that it is enough to use the initial contact points, because the object won't
// have "slipped" after being attached, and the location of the original contact point
// on the link itself is considered as a fixed point on the link, regardless whether this
// point is currently still colliding with the object or not. We only want to know whether
// this fixed point on the link has increased in distance from the corresponding fixed
// point (where the contact originally happened) on the object.
std::map<std::string, std::map<std::string, CollidingPoint> >::iterator
initCollIt = this->attachGripContacts.find(objName);
if (initCollIt == this->attachGripContacts.end())
{
std::cerr << "ERROR: Consistency: Could not find attachGripContacts for " <<
objName << std::endl;
continue;
}
std::map<std::string, CollidingPoint> &initColls = initCollIt->second;
int cntRelease = 0;
// for all links which have initially been detected to collide:
std::map<std::string, CollidingPoint>::iterator pointIt;
for (pointIt = initColls.begin(); pointIt != initColls.end(); ++pointIt)
{
CollidingPoint &cpInfo = pointIt->second;
// initial distance from link to contact point (relative to link)
GzVector3 relContactPos = cpInfo.pos / cpInfo.sum;
// Initial distance from link to object (relative to link)
GzVector3 relObjPos = cpInfo.objPos / cpInfo.sum;
// Get current world pose of object
GzPose3 currObjWorldPose =
gazebo::GetWorldPose(cpInfo.collObj->GetLink());
// Get world pose of link
GzPose3 currLinkWorldPose =
gazebo::GetWorldPose(cpInfo.collLink->GetLink());
// Get transform for currLinkWorldPose as matrix
GzMatrix4 worldToLink = gazebo::GetMatrix(currLinkWorldPose);
// Get the transform from collision link to contact point
GzMatrix4 linkToContact = gazebo::GetMatrix(relContactPos);
// The current world position of the contact point right now is:
GzMatrix4 _currContactWorldPose = worldToLink * linkToContact;
GzVector3 currContactWorldPose = gazebo::GetPos(_currContactWorldPose);
// The initial contact point location on the link should still correspond
// to the initial contact point location on the object.
// Initial vector from object center to contact point (relative to link,
// because relObjPos and relContactPos are from center of link)
GzVector3 oldObjDist = relContactPos - relObjPos;
// The same vector as \e oldObjDist, but calculated by the current world pose
// of object and the current location of the initial contact location on the link.
// This is the new distance from contact to object.
GzVector3 newObjDist = currContactWorldPose - gazebo::GetPos(currObjWorldPose);
//gzmsg<<"Obj Trans "<<cpInfo.collLink->GetName()<<": "<<relObjPos.x<<", "<<relObjPos.y<<", "<<relObjPos.z<<std::endl;
//gzmsg<<"Cont Trans "<<cpInfo.collLink->GetName()<<": "<<relContactPos.x<<", "<<relContactPos.y<<", "<<relContactPos.z<<std::endl;
// the difference between these vectors should not be too large...
float diff =
fabs(gazebo::GetLength(oldObjDist) - gazebo::GetLength(newObjDist));
//gzmsg<<"Diff for link "<<cpInfo.collLink->GetName()<<": "<<diff<<std::endl;
if (diff > releaseTolerance)
{
++cntRelease;
}
}
if (cntRelease > 0)
{
// sufficient links did not meet the criteria to be close enough to the object.
// First, get the grasping gripper:
std::map<std::string, GazeboGraspGripper>::iterator gggIt = grippers.find(
graspingGripperName);
if (gggIt == grippers.end())
{
gzerr << "GazeboGraspFix: Inconsistency: Gazebo gripper '" <<
graspingGripperName << "' not found when checking for detachment" << std::endl;
continue;
}
GazeboGraspGripper &graspingGripper = gggIt->second;
// Now, detach the object:
gzmsg << "GazeboGraspFix: Detaching " << objName << " from gripper " <<
graspingGripperName << "." << std::endl;
graspingGripper.HandleDetach(objName);
this->OnDetach(objName, graspingGripperName);
gripCntIt->second = 0;
this->attachGripContacts.erase(initCollIt);
}
}
this->prevUpdateTime = common::Time::GetWallTime();
}
////////////////////////////////////////////////////////////////////////////////
void GazeboGraspFix::OnContact(const ConstContactsPtr &_msg)
{
//gzmsg<<"CONTACT! "<<std::endl;//<<_contact<<std::endl;
// for all contacts...
for (int i = 0; i < _msg->contact_size(); ++i)
{
physics::CollisionPtr collision1 =
boost::dynamic_pointer_cast<physics::Collision>(
gazebo::GetEntityByName(this->world, _msg->contact(i).collision1()));
physics::CollisionPtr collision2 =
boost::dynamic_pointer_cast<physics::Collision>(
gazebo::GetEntityByName(this->world, _msg->contact(i).collision2()));
if ((collision1 && !collision1->IsStatic()) && (collision2
&& !collision2->IsStatic()))
{
std::string name1 = collision1->GetScopedName();
std::string name2 = collision2->GetScopedName();
//gzmsg<<"OBJ CONTACT! "<<name1<<" / "<<name2<<std::endl;
int count = _msg->contact(i).position_size();
// Check to see if the contact arrays all have the same size.
if ((count != _msg->contact(i).normal_size()) ||
(count != _msg->contact(i).wrench_size()) ||
(count != _msg->contact(i).depth_size()))
{
gzerr << "GazeboGraspFix: Contact message has invalid array sizes\n" <<
std::endl;
continue;
}
std::string collidingObjName, collidingLink, gripperOfCollidingLink;
physics::CollisionPtr linkCollision;
physics::CollisionPtr objCollision;
physics::Contact contact;
contact = _msg->contact(i);
if (contact.count < 1)
{
std::cerr << "ERROR: GazeboGraspFix: Not enough forces given for contact of ."
<< name1 << " / " << name2 << std::endl;
continue;
}
// all force vectors which are part of this contact
std::vector<GzVector3> force;
// find out which part of the colliding entities is the object, *not* the gripper,
// and copy all the forces applied to it into the vector 'force'.
std::map<std::string, std::string>::const_iterator gripperCollIt =
this->collisions.find(name2);
if (gripperCollIt != this->collisions.end())
{
// collision 1 is the object
collidingObjName = name1;
collidingLink = name2;
linkCollision = collision2;
objCollision = collision1;
gripperOfCollidingLink = gripperCollIt->second;
for (int k = 0; k < contact.count; ++k)
force.push_back(contact.wrench[k].body1Force);
}
else if ((gripperCollIt = this->collisions.find(name1)) !=
this->collisions.end())
{
// collision 2 is the object
collidingObjName = name2;
collidingLink = name1;
linkCollision = collision1;
objCollision = collision2;
gripperOfCollidingLink = gripperCollIt->second;
for (int k = 0; k < contact.count; ++k)
force.push_back(contact.wrench[k].body2Force);
}
GzVector3 avgForce;
// compute average/sum of the forces applied on the object
for (int k = 0; k < force.size(); ++k)
{
avgForce += force[k];
}
avgForce /= force.size();
GzVector3 avgPos;
// compute center point (average pose) of all the origin positions of the forces appied
for (int k = 0; k < contact.count; ++k) avgPos += contact.positions[k];
avgPos /= contact.count;
// now, get average pose relative to the colliding link
GzPose3 linkWorldPose = gazebo::GetWorldPose(linkCollision->GetLink());
// To find out the collision point relative to the Link's local coordinate system, first get the Poses as 4x4 matrices
GzMatrix4 worldToLink = gazebo::GetMatrix(linkWorldPose);
// We can assume that the contact has identity rotation because we don't care about its orientation.
// We could always set another rotation here too.
GzMatrix4 worldToContact = gazebo::GetMatrix(avgPos);
// now, worldToLink * contactInLocal = worldToContact
// hence, contactInLocal = worldToLink.Inv * worldToContact
GzMatrix4 worldToLinkInv = worldToLink.Inverse();
GzMatrix4 contactInLocal = worldToLinkInv * worldToContact;
GzVector3 contactPosInLocal = gazebo::GetPos(contactInLocal);
//gzmsg<<"---------"<<std::endl;
//gzmsg<<"CNT in loc: "<<contactPosInLocal.x<<","<<contactPosInLocal.y<<","<<contactPosInLocal.z<<std::endl;
/*GzVector3 sDiff=avgPos-linkWorldPose.pos;
gzmsg<<"SIMPLE trans: "<<sDiff.x<<","<<sDiff.y<<","<<sDiff.z<<std::endl;
gzmsg<<"coll world pose: "<<linkWorldPose.pos.x<<", "<<linkWorldPose.pos.y<<", "<<linkWorldPose.pos.z<<std::endl;
gzmsg<<"contact avg pose: "<<avgPos.x<<", "<<avgPos.y<<", "<<avgPos.z<<std::endl;
GzVector3 lX=linkWorldPose.rot.GetXAxis();
GzVector3 lY=linkWorldPose.rot.GetYAxis();
GzVector3 lZ=linkWorldPose.rot.GetZAxis();
gzmsg<<"World ori: "<<linkWorldPose.rot.x<<","<<linkWorldPose.rot.y<<","<<linkWorldPose.rot.z<<","<<linkWorldPose.rot.w<<std::endl;
gzmsg<<"x axis: "<<lX.x<<","<<lX.y<<","<<lX.z<<std::endl;
gzmsg<<"y axis: "<<lY.x<<","<<lY.y<<","<<lY.z<<std::endl;
gzmsg<<"z axis: "<<lZ.x<<","<<lZ.y<<","<<lZ.z<<std::endl;*/
// Get the pose of the object and compute it's relative position to
// the collision surface.
GzPose3 objWorldPose = gazebo::GetWorldPose(objCollision->GetLink());
GzMatrix4 worldToObj = gazebo::GetMatrix(objWorldPose);
GzMatrix4 objInLocal = worldToLinkInv * worldToObj;
GzVector3 objPosInLocal = gazebo::GetPos(objInLocal);
{
boost::mutex::scoped_lock lock(this->mutexContacts);
CollidingPoint &p =
this->contacts[collidingObjName][collidingLink]; // inserts new entry if doesn't exist
p.gripperName = gripperOfCollidingLink;
p.collLink = linkCollision;
p.collObj = objCollision;
p.force += avgForce;
p.pos += contactPosInLocal;
p.objPos += objPosInLocal;
p.sum++;
}
//gzmsg<<"Average force of contact= "<<avgForce.x<<", "<<avgForce.y<<", "<<avgForce.z<<" out of "<<force.size()<<" vectors."<<std::endl;
}
}
}
void GazeboGraspFix::OnAttach(const std::string &objectName,
const std::string &armName)
{
msgs::GraspEvent event;
event.set_arm(armName);
event.set_object(objectName);
event.set_attached(true);
eventsPub->Publish(event);
}
void GazeboGraspFix::OnDetach(const std::string &objectName,
const std::string &armName)
{
msgs::GraspEvent event;
event.set_arm(armName);
event.set_object(objectName);
event.set_attached(false);
eventsPub->Publish(event);
}

265
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin/src/GazeboGraspGripper.cpp
View file

@ -1,265 +0,0 @@
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/physics/ContactManager.hh>
#include <gazebo/physics/Contact.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>
#include <gazebo_version_helpers/GazeboVersionHelpers.h>
using gazebo::GazeboGraspGripper;
#define DEFAULT_FORCES_ANGLE_TOLERANCE 120
#define DEFAULT_UPDATE_RATE 5
#define DEFAULT_MAX_GRIP_COUNT 10
#define DEFAULT_RELEASE_TOLERANCE 0.005
#define DEFAULT_DISABLE_COLLISIONS_ON_ATTACH false
///////////////////////////////////////////////////////////////////////////////
GazeboGraspGripper::GazeboGraspGripper():
attached(false)
{
}
///////////////////////////////////////////////////////////////////////////////
GazeboGraspGripper::GazeboGraspGripper(const GazeboGraspGripper &o):
model(o.model),
gripperName(o.gripperName),
linkNames(o.linkNames),
collisionElems(o.collisionElems),
fixedJoint(o.fixedJoint),
palmLink(o.palmLink),
disableCollisionsOnAttach(o.disableCollisionsOnAttach),
attached(o.attached),
attachedObjName(o.attachedObjName)
{}
///////////////////////////////////////////////////////////////////////////////
GazeboGraspGripper::~GazeboGraspGripper()
{
this->model.reset();
}
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::Init(physics::ModelPtr &_model,
const std::string &_gripperName,
const std::string &palmLinkName,
const std::vector<std::string> &fingerLinkNames,
bool _disableCollisionsOnAttach,
std::map<std::string, physics::CollisionPtr> &_collisionElems)
{
this->gripperName = _gripperName;
this->attached = false;
this->disableCollisionsOnAttach = _disableCollisionsOnAttach;
this->model = _model;
physics::PhysicsEnginePtr physics =
gazebo::GetPhysics(this->model->GetWorld());
this->fixedJoint = physics->CreateJoint("revolute");
this->palmLink = this->model->GetLink(palmLinkName);
if (!this->palmLink)
{
gzerr << "GazeboGraspGripper: Palm link " << palmLinkName <<
" not found. The gazebo grasp plugin will not work." << std::endl;
return false;
}
for (std::vector<std::string>::const_iterator fingerIt =
fingerLinkNames.begin();
fingerIt != fingerLinkNames.end(); ++fingerIt)
{
physics::LinkPtr link = this->model->GetLink(*fingerIt);
//gzmsg<<"Got link "<<fingerLinkElem->Get<std::string>()<<std::endl;
if (!link.get())
{
gzerr << "GazeboGraspGripper ERROR: Link " << *fingerIt <<
" can't be found in gazebo for GazeboGraspGripper model plugin. Skipping." <<
std::endl;
continue;
}
for (unsigned int j = 0; j < link->GetChildCount(); ++j)
{
physics::CollisionPtr collision = link->GetCollision(j);
std::string collName = collision->GetScopedName();
//collision->SetContactsEnabled(true);
std::map<std::string, physics::CollisionPtr>::iterator collIter =
collisionElems.find(collName);
if (collIter !=
this->collisionElems.end()) //this collision was already added before
{
gzwarn << "GazeboGraspGripper: Adding Gazebo collision link element " <<
collName << " multiple times, the gazebo grasp handler may not work properly" <<
std::endl;
continue;
}
this->collisionElems[collName] = collision;
_collisionElems[collName] = collision;
}
}
return !this->collisionElems.empty();
}
///////////////////////////////////////////////////////////////////////////////
const std::string &GazeboGraspGripper::getGripperName() const
{
return gripperName;
}
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::hasLink(const std::string &linkName) const
{
for (std::vector<std::string>::const_iterator it = linkNames.begin();
it != linkNames.end(); ++it)
{
if (*it == linkName) return true;
}
return false;
}
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::hasCollisionLink(const std::string &linkName) const
{
return collisionElems.find(linkName) != collisionElems.end();
}
///////////////////////////////////////////////////////////////////////////////
bool GazeboGraspGripper::isObjectAttached() const
{
return attached;
}
///////////////////////////////////////////////////////////////////////////////
const std::string &GazeboGraspGripper::attachedObject() const
{
return attachedObjName;
}
///////////////////////////////////////////////////////////////////////////////
// #define USE_MODEL_ATTACH // this only works if the object is a model in itself, which is usually not
// the case. Leaving this in here anyway for documentation of what has been
// tried, and for and later re-evaluation.
bool GazeboGraspGripper::HandleAttach(const std::string &objName)
{
if (!this->palmLink)
{
gzwarn << "GazeboGraspGripper: palm link not found, not enforcing grasp hack!\n"
<< std::endl;
return false;
}
physics::WorldPtr world = this->model->GetWorld();
if (!world.get())
{
gzerr << "GazeboGraspGripper: world is NULL" << std::endl;
return false;
}
#ifdef USE_MODEL_ATTACH
physics::ModelPtr obj = world->GetModel(objName);
if (!obj.get())
{
std::cerr << "ERROR: Object ModelPtr " << objName <<
" not found in world, can't attach it" << std::endl;
return false;
}
gazebo::math::Pose diff = obj->GetLink()->GetWorldPose() -
this->palmLink->GetWorldPose();
this->palmLink->AttachStaticModel(obj, diff);
#else
physics::CollisionPtr obj =
boost::dynamic_pointer_cast<physics::Collision>(gazebo::GetEntityByName(world, objName));
if (!obj.get())
{
std::cerr << "ERROR: Object " << objName <<
" not found in world, can't attach it" << std::endl;
return false;
}
gazebo::GzPose3 diff = gazebo::GetWorldPose(obj->GetLink()) -
gazebo::GetWorldPose(this->palmLink);
this->fixedJoint->Load(this->palmLink, obj->GetLink(), diff);
this->fixedJoint->Init();
#if GAZEBO_MAJOR_VERSION >= 8
this->fixedJoint->SetUpperLimit(0, 0);
this->fixedJoint->SetLowerLimit(0, 0);
#else
this->fixedJoint->SetHighStop(0, 0);
this->fixedJoint->SetLowStop(0, 0);
#endif
if (this->disableCollisionsOnAttach)
{
// we can disable collisions of the grasped object, because when the fingers keep colliding with
// it, the fingers keep wobbling, which can create difficulties when moving the arm.
obj->GetLink()->SetCollideMode("none");
}
#endif // USE_MODEL_ATTACH
this->attached = true;
this->attachedObjName = objName;
return true;
}
///////////////////////////////////////////////////////////////////////////////
void GazeboGraspGripper::HandleDetach(const std::string &objName)
{
physics::WorldPtr world = this->model->GetWorld();
if (!world.get())
{
gzerr << "GazeboGraspGripper: world is NULL" << std::endl << std::endl;
return;
}
#ifdef USE_MODEL_ATTACH
physics::ModelPtr obj = world->GetModel(objName);
if (!obj.get())
{
std::cerr << "ERROR: Object ModelPtr " << objName <<
" not found in world, can't detach it" << std::endl;
return;
}
this->palmLink->DetachStaticModel(objName);
#else
physics::CollisionPtr obj = boost::dynamic_pointer_cast<physics::Collision>
(gazebo::GetEntityByName(world, objName));
if (!obj.get())
{
std::cerr << "ERROR: Object " << objName <<
" not found in world, can't attach it" << std::endl;
return;
}
else if (this->disableCollisionsOnAttach)
{
obj->GetLink()->SetCollideMode("all");
}
// TODO: remove this test print, for issue #26 -------------------
#if 0
if (obj && obj->GetLink())
{
auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
this->fixedJoint->Detach();
// TODO: remove this test print, for issue #26 -------------------
#if 0
if (obj && obj->GetLink())
{
auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
#endif // USE_MODEL_ATTACH
this->attached = false;
this->attachedObjName = "";
}

83
mycobot_320m5_gazebo/gazebo-pkgs/gazebo_grasp_plugin_ros/CMakeLists.txt
View file

@ -1,83 +0,0 @@
cmake_minimum_required(VERSION 3.0.2)
project(gazebo_grasp_plugin_ros)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
gazebo_grasp_plugin
message_generation
roscpp
)
find_package(gazebo REQUIRED)
################################################
## Messages
################################################
add_message_files(
FILES GazeboGraspEvent.msg
)
generate_messages(
DEPENDENCIES std_msgs
)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if your package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES gazebo_grasp_plugin_ros
CATKIN_DEPENDS gazebo_grasp_plugin message_runtime roscpp
DEPENDS gazebo
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
${catkin_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)
link_directories(
${catkin_LIBRARY_DIRS}
${GAZEBO_LIBRARY_DIRS}
)
## Declare a C++ executable
add_executable(${PROJECT_NAME}_republisher_node src/grasp_event_republisher.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_republisher_node"
set_target_properties(${PROJECT_NAME}_republisher_node PROPERTIES OUTPUT_NAME grasp_event_republisher PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_republisher_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_republisher_node gazebo_grasp_plugin_ros_generate_messages_cpp)
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_republisher_node
${catkin_LIBRARIES}
${GAZEBO_LIBRARIES}
)
#############
## Install ##
#############
## Mark executables for installation
install(TARGETS ${PROJECT_NAME}_republisher_node
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

Some files were not shown because too many files have changed in this diff Show more