Skip to content

Commit

Permalink
add 280jn gripper moveit
Browse files Browse the repository at this point in the history
  • Loading branch information
wangWking committed Sep 12, 2024
1 parent 70e59b5 commit d9de0f5
Show file tree
Hide file tree
Showing 45 changed files with 2,228 additions and 0 deletions.
11 changes: 11 additions & 0 deletions mycobot_280/mycobot_280jn_gripper_moveit/.setup_assistant
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
moveit_setup_assistant_config:
URDF:
package: mycobot_description
relative_path: urdf/mycobot_280_jn/mycobot_280_jn_adaptive_gripper_parallel.urdf
xacro_args: ""
SRDF:
relative_path: config/firefighter.srdf
CONFIG:
author_name: wwj
author_email: [email protected]
generated_timestamp: 1726133132
15 changes: 15 additions & 0 deletions mycobot_280/mycobot_280jn_gripper_moveit/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.1.3)
project(mycobot_280jn_gripper_moveit)

find_package(catkin REQUIRED)

catkin_package()

catkin_install_python(PROGRAMS
scripts/sync_plan.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
PATTERN "setup_assistant.launch" EXCLUDE)
install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
cartesian_limits:
max_trans_vel: 1
max_trans_acc: 2.25
max_trans_dec: -5
max_rot_vel: 1.57
Original file line number Diff line number Diff line change
@@ -0,0 +1,18 @@
planning_time_limit: 10.0
max_iterations: 200
max_iterations_after_collision_free: 5
smoothness_cost_weight: 0.1
obstacle_cost_weight: 1.0
learning_rate: 0.01
smoothness_cost_velocity: 0.0
smoothness_cost_acceleration: 1.0
smoothness_cost_jerk: 0.0
ridge_factor: 0.0
use_pseudo_inverse: false
pseudo_inverse_ridge_factor: 1e-4
joint_update_limit: 0.1
collision_clearance: 0.2
collision_threshold: 0.07
use_stochastic_descent: true
enable_failure_recovery: false
max_recovery_attempts: 5
Original file line number Diff line number Diff line change
@@ -0,0 +1,17 @@
controller_list:
- name: fake_arm_group_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_group_controller
type: $(arg fake_execution_type)
joints:
- gripper_controller
initial: # Define initial robot poses per group
- group: arm_group
pose: init_pose
142 changes: 142 additions & 0 deletions mycobot_280/mycobot_280jn_gripper_moveit/config/firefighter.srdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,142 @@
<?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_group">
<joint name="joint2_to_joint1"/>
<joint name="joint3_to_joint2"/>
<joint name="joint4_to_joint3"/>
<joint name="joint5_to_joint4"/>
<joint name="joint6_to_joint5"/>
<joint name="joint6output_to_joint6"/>
<chain base_link="g_base" tip_link="gripper_base"/>
</group>
<group name="gripper">
<chain base_link="gripper_base" tip_link="gripper_left3"/>
</group>
<!--GROUP STATES: Purpose: Define a named state for a particular group, in terms of joint values. This is useful to define states like 'folded arms'-->
<group_state name="init_pose" group="arm_group">
<joint name="joint2_to_joint1" value="0"/>
<joint name="joint3_to_joint2" value="0"/>
<joint name="joint4_to_joint3" value="0"/>
<joint name="joint5_to_joint4" value="0"/>
<joint name="joint6_to_joint5" value="0"/>
<joint name="joint6output_to_joint6" value="0"/>
</group_state>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="g_base"/>
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<passive_joint name="gripper_right3_to_gripper_right1"/>
<passive_joint name="gripper_left3_to_gripper_left1"/>
<passive_joint name="gripper_base_to_gripper_right2"/>
<passive_joint name="gripper_base_to_gripper_right3"/>
<passive_joint name="gripper_base_to_gripper_left2"/>
<!--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="g_base" link2="gripper_base" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_left1" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_left2" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_left3" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_right1" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_right2" reason="Default"/>
<disable_collisions link1="g_base" link2="gripper_right3" reason="Default"/>
<disable_collisions link1="g_base" link2="joint1" reason="Adjacent"/>
<disable_collisions link1="g_base" link2="joint2" reason="Never"/>
<disable_collisions link1="g_base" link2="joint3" reason="Never"/>
<disable_collisions link1="g_base" link2="joint4" reason="Never"/>
<disable_collisions link1="g_base" link2="joint5" reason="Default"/>
<disable_collisions link1="g_base" link2="joint6" reason="Never"/>
<disable_collisions link1="g_base" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="joint1" link2="joint2" reason="Adjacent"/>
<disable_collisions link1="joint1" link2="joint3" reason="Never"/>
<disable_collisions link1="joint1" link2="joint4" reason="Never"/>
<disable_collisions link1="joint1" link2="joint5" reason="Default"/>
<disable_collisions link1="joint1" link2="joint6" reason="Never"/>
<disable_collisions link1="joint1" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="joint2" link2="joint3" reason="Adjacent"/>
<disable_collisions link1="joint2" link2="joint5" reason="Never"/>
<disable_collisions link1="joint3" link2="joint4" reason="Adjacent"/>
<disable_collisions link1="joint3" link2="joint5" reason="Never"/>
<disable_collisions link1="joint4" link2="joint5" reason="Adjacent"/>
<disable_collisions link1="joint4" link2="joint6" reason="Never"/>
<disable_collisions link1="joint5" link2="joint6" reason="Adjacent"/>
<disable_collisions link1="joint5" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="joint6" link2="joint6_flange" reason="Adjacent"/>
<disable_collisions link1="gripper_base" link2="gripper_left1" reason="Never"/>
<disable_collisions link1="gripper_base" link2="gripper_left2" reason="Adjacent"/>
<disable_collisions link1="gripper_base" link2="gripper_left3" reason="Adjacent"/>
<disable_collisions link1="gripper_base" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="gripper_base" link2="gripper_right2" reason="Adjacent"/>
<disable_collisions link1="gripper_base" link2="gripper_right3" reason="Adjacent"/>
<disable_collisions link1="gripper_base" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_base" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_base" link2="joint6_flange" reason="Adjacent"/>

<disable_collisions link1="gripper_base" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_base" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_base" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_base" link2="joint4" reason="Never"/>

<disable_collisions link1="gripper_left1" link2="gripper_left2" reason="Default"/>
<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="joint1" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_left1" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="gripper_left3" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="gripper_right2" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_left2" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="gripper_right1" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_left3" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="gripper_right2" reason="Default"/>
<disable_collisions link1="gripper_right1" link2="gripper_right3" reason="Adjacent"/>
<disable_collisions link1="gripper_right1" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_right1" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="gripper_right3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_right2" link2="joint6_flange" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint1" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint2" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint3" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint4" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint5" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint6" reason="Never"/>
<disable_collisions link1="gripper_right3" link2="joint6_flange" reason="Never"/>

</robot>
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
# Publish joint_states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
Loading

0 comments on commit d9de0f5

Please sign in to comment.