diff --git a/ur10_moveit_config/.setup_assistant b/ur10_moveit_config/.setup_assistant
index d9226682110f0eacb8b5f8b268b6c93e26c784c1..026a76bc594333c47ad87c4c1a5fe7b0ade1a185 100644
--- a/ur10_moveit_config/.setup_assistant
+++ b/ur10_moveit_config/.setup_assistant
@@ -5,4 +5,4 @@ moveit_setup_assistant_config:
SRDF:
relative_path: config/ur10.srdf
CONFIG:
- generated_timestamp: 1405109015
\ No newline at end of file
+ generated_timestamp: 1409148189
\ No newline at end of file
diff --git a/ur10_moveit_config/config/kinematics.yaml b/ur10_moveit_config/config/kinematics.yaml
index 969ba14252bfe9c5dc61a6a01a078447e08f69bb..0ee1e5af01cbe20c2991f329208a723816cc2da6 100644
--- a/ur10_moveit_config/config/kinematics.yaml
+++ b/ur10_moveit_config/config/kinematics.yaml
@@ -1,5 +1,10 @@
manipulator:
- kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+ kinematics_solver: ur_kinematics/UR10KinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
\ No newline at end of file
+ kinematics_solver_attempts: 3
+#manipulator:
+# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+# kinematics_solver_search_resolution: 0.005
+# kinematics_solver_timeout: 0.005
+# kinematics_solver_attempts: 3
\ No newline at end of file
diff --git a/ur10_moveit_config/config/ur10.srdf b/ur10_moveit_config/config/ur10.srdf
index fd7750bec8e70d6e96322d8a83aa984f60c0efa9..d943a92ca160934284b24c390e83bf07b6db47ec 100644
--- a/ur10_moveit_config/config/ur10.srdf
+++ b/ur10_moveit_config/config/ur10.srdf
@@ -35,15 +35,18 @@
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="moveit_ee" parent_link="ee_link" group="endeffector" />
<!--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="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />
<!--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_link" link2="forearm_link" reason="Never" />
<disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
<disable_collisions link1="base_link" link2="upper_arm_link" reason="Never" />
- <disable_collisions link1="base_link" link2="wrist_1_link" reason="Never" />
+ <disable_collisions link1="forearm_link" link2="shoulder_link" reason="Never" />
<disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
<disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
+ <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never" />
+ <disable_collisions link1="forearm_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
- <disable_collisions link1="shoulder_link" link2="wrist_1_link" reason="Never" />
- <disable_collisions link1="shoulder_link" link2="wrist_2_link" reason="Never" />
+ <disable_collisions link1="upper_arm_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
<disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
diff --git a/ur10_moveit_config/launch/moveit.rviz b/ur10_moveit_config/launch/moveit.rviz
index 80f5d624bfb802d51aeaa479df6fd1fa52bc01d1..617af724f275151044074a9e602518d96b3b412f 100644
--- a/ur10_moveit_config/launch/moveit.rviz
+++ b/ur10_moveit_config/launch/moveit.rviz
@@ -6,7 +6,7 @@ Panels:
Expanded:
- /MotionPlanning1
Splitter Ratio: 0.74256
- Tree Height: 664
+ Tree Height: 291
- Class: rviz/Help
Name: Help
- Class: rviz/Views
@@ -45,282 +45,50 @@ Visualization Manager:
Name: MotionPlanning
Planned Path:
Links:
- base_bellow_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_footprint:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- double_stereo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
+ ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_roll_link:
+ forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_link:
+ shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_tip_link:
+ upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_motor_accelerometer_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_gripper_palm_link:
+ wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_link:
+ wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- laser_tilt_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- sensor_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- torso_lift_link:
+ wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
@@ -338,13 +106,14 @@ Visualization Manager:
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
+ TextHeight: 0.08
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
- Planning Group: left_arm
+ Planning Group: manipulator
Query Goal State: true
Query Start State: false
Show Workspace: false
@@ -362,293 +131,63 @@ Visualization Manager:
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
- base_bellow_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_footprint:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- double_stereo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
+ ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_forearm_roll_link:
+ forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_link:
+ shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_tip_link:
+ upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_motor_accelerometer_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_gripper_palm_link:
+ wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_link:
+ wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- laser_tilt_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- sensor_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- torso_lift_link:
+ wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
- Show Scene Robot: true
+ Show Robot Collision: false
+ Show Robot Visual: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: /base_link
+ Fixed Frame: world
+ Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
@@ -660,30 +199,35 @@ Visualization Manager:
Current:
Class: rviz/XYOrbit
Distance: 2.9965
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
Focal Point:
- X: 0.113567
- Y: 0.10592
- Z: 2.23518e-07
+ X: 0.747697
+ Y: -0.20044
+ Z: 3.42727e-07
Name: Current View
Near Clip Distance: 0.01
- Pitch: 0.509797
- Target Frame: /base_link
+ Pitch: 0.289797
+ Target Frame: world
Value: XYOrbit (rviz)
- Yaw: 5.65995
+ Yaw: 5.44495
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1337
+ Height: 876
Help:
collapsed: false
Hide Left Dock: false
Hide Right Dock: false
Motion Planning:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000005fb000000100044006900730070006c0061007900730100000036000001b4000000b700fffffffb0000000800480065006c00700000000342000000bb0000006500fffffffb0000000a0056006900650077007300000003b0000000b00000009b00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001f0000001620000015700ffffff000003570000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
- Width: 1828
- X: 459
- Y: -243
+ Width: 1535
+ X: 65
+ Y: 24
diff --git a/ur10_moveit_config/package.xml b/ur10_moveit_config/package.xml
index ed95a00e9e610358db40423b6be43ce9d15e8b02..fe87af34a3e31b601db47b60ce86acc722e43e66 100644
--- a/ur10_moveit_config/package.xml
+++ b/ur10_moveit_config/package.xml
@@ -1,7 +1,7 @@
<package>
<name>ur10_moveit_config</name>
- <version>1.1.0</version>
+ <version>1.1.1</version>
<description>
An automatically generated package with all the configuration and launch files for using the ur10 with the MoveIt Motion Planning Framework
</description>
diff --git a/ur5_moveit_config/.setup_assistant b/ur5_moveit_config/.setup_assistant
index da441fc4e803b8ec4b31a21aec25365775cfe0e5..0a983ecc22b7103e872b28242ebe99b26176e18c 100644
--- a/ur5_moveit_config/.setup_assistant
+++ b/ur5_moveit_config/.setup_assistant
@@ -5,4 +5,4 @@ moveit_setup_assistant_config:
SRDF:
relative_path: config/ur5.srdf
CONFIG:
- generated_timestamp: 1395340824
\ No newline at end of file
+ generated_timestamp: 1409142284
\ No newline at end of file
diff --git a/ur5_moveit_config/config/kinematics.yaml b/ur5_moveit_config/config/kinematics.yaml
index 969ba14252bfe9c5dc61a6a01a078447e08f69bb..5ed94f2a4df1df20ed143f9a9b2b3b9c2d15493e 100644
--- a/ur5_moveit_config/config/kinematics.yaml
+++ b/ur5_moveit_config/config/kinematics.yaml
@@ -1,5 +1,10 @@
manipulator:
- kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+ kinematics_solver: ur_kinematics/UR5KinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
- kinematics_solver_attempts: 3
\ No newline at end of file
+ kinematics_solver_attempts: 3
+#manipulator:
+# kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+# kinematics_solver_search_resolution: 0.005
+# kinematics_solver_timeout: 0.005
+# kinematics_solver_attempts: 3
\ No newline at end of file
diff --git a/ur5_moveit_config/config/ur5.srdf b/ur5_moveit_config/config/ur5.srdf
index d2180f69be8a656a568f1ae28ac46a45f0f39c1b..9b6db329a015b288452bf5ad60b71bbfcb45379d 100644
--- a/ur5_moveit_config/config/ur5.srdf
+++ b/ur5_moveit_config/config/ur5.srdf
@@ -37,10 +37,16 @@
<!--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="fixed_base" type="fixed" parent_frame="world" child_link="base_link" />
<!--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_link" link2="forearm_link" reason="Never" />
<disable_collisions link1="base_link" link2="shoulder_link" reason="Adjacent" />
+ <disable_collisions link1="base_link" link2="upper_arm_link" reason="Never" />
+ <disable_collisions link1="forearm_link" link2="shoulder_link" reason="Never" />
<disable_collisions link1="forearm_link" link2="upper_arm_link" reason="Adjacent" />
<disable_collisions link1="forearm_link" link2="wrist_1_link" reason="Adjacent" />
+ <disable_collisions link1="forearm_link" link2="wrist_2_link" reason="Never" />
+ <disable_collisions link1="forearm_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="shoulder_link" link2="upper_arm_link" reason="Adjacent" />
+ <disable_collisions link1="upper_arm_link" link2="wrist_1_link" reason="Never" />
<disable_collisions link1="wrist_1_link" link2="wrist_2_link" reason="Adjacent" />
<disable_collisions link1="wrist_1_link" link2="wrist_3_link" reason="Never" />
<disable_collisions link1="wrist_2_link" link2="wrist_3_link" reason="Adjacent" />
diff --git a/ur5_moveit_config/launch/moveit.rviz b/ur5_moveit_config/launch/moveit.rviz
index 80f5d624bfb802d51aeaa479df6fd1fa52bc01d1..c7535556e97c2286f95983a0294cd6459348b9f8 100644
--- a/ur5_moveit_config/launch/moveit.rviz
+++ b/ur5_moveit_config/launch/moveit.rviz
@@ -6,7 +6,7 @@ Panels:
Expanded:
- /MotionPlanning1
Splitter Ratio: 0.74256
- Tree Height: 664
+ Tree Height: 291
- Class: rviz/Help
Name: Help
- Class: rviz/Views
@@ -45,282 +45,50 @@ Visualization Manager:
Name: MotionPlanning
Planned Path:
Links:
- base_bellow_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_footprint:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- double_stereo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
+ ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_roll_link:
+ forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_link:
+ shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_tip_link:
+ upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_motor_accelerometer_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_gripper_palm_link:
+ wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_link:
+ wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- laser_tilt_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- sensor_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- torso_lift_link:
+ wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
@@ -338,13 +106,14 @@ Visualization Manager:
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
+ TextHeight: 0.08
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
- Planning Group: left_arm
+ Planning Group: manipulator
Query Goal State: true
Query Start State: false
Show Workspace: false
@@ -362,293 +131,63 @@ Visualization Manager:
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
- base_bellow_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_footprint:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- bl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- bl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- br_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- double_stereo_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fl_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_l_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_r_wheel_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- fr_caster_rotation_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_kinect_rgb_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_mount_prosilica_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_plate_frame:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- head_tilt_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_forearm_link:
+ ee_link:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_forearm_roll_link:
+ forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_link:
+ shoulder_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_l_finger_tip_link:
+ upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_motor_accelerometer_link:
+ world:
Alpha: 1
Show Axes: false
Show Trail: false
- Value: true
- l_gripper_palm_link:
+ wrist_1_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_link:
+ wrist_2_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
- l_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- l_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- laser_tilt_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_elbow_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_forearm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_l_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_motor_accelerometer_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_palm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_gripper_r_finger_tip_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_lift_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_shoulder_pan_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_upper_arm_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_flex_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- r_wrist_roll_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- sensor_mount_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- torso_lift_link:
+ wrist_3_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
- Show Scene Robot: true
+ Show Robot Collision: false
+ Show Robot Visual: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
- Fixed Frame: /base_link
+ Fixed Frame: world
+ Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
@@ -660,6 +199,11 @@ Visualization Manager:
Current:
Class: rviz/XYOrbit
Distance: 2.9965
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.06
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
Focal Point:
X: 0.113567
Y: 0.10592
@@ -667,23 +211,23 @@ Visualization Manager:
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.509797
- Target Frame: /base_link
+ Target Frame: world
Value: XYOrbit (rviz)
Yaw: 5.65995
Saved: ~
Window Geometry:
Displays:
collapsed: false
- Height: 1337
+ Height: 876
Help:
collapsed: false
Hide Left Dock: false
Hide Right Dock: false
Motion Planning:
collapsed: false
- QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000100000000000002a20000031cfc0200000005fb000000100044006900730070006c0061007900730100000036000001b4000000b700fffffffb0000000800480065006c00700000000342000000bb0000006500fffffffb0000000a0056006900650077007300000003b0000000b00000009b00fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e006701000001f0000001620000015700ffffff000003570000031c00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
- Width: 1828
- X: 459
- Y: -243
+ Width: 1535
+ X: 65
+ Y: 24
diff --git a/ur5_moveit_config/package.xml b/ur5_moveit_config/package.xml
index 38abf305c49766bf24d174bdede1e5c8190e4e9a..110f8814e1134386b895557472b9f06457defd01 100644
--- a/ur5_moveit_config/package.xml
+++ b/ur5_moveit_config/package.xml
@@ -1,7 +1,7 @@
<package>
<name>ur5_moveit_config</name>
- <version>1.0.2</version>
+ <version>1.0.3</version>
<description>
An automatically generated package with all the configuration and launch files for using the ur5 with the MoveIt Motion Planning Framework
</description>
diff --git a/ur_description/urdf/ur10_joint_limited.urdf.xacro b/ur_description/urdf/ur10_joint_limited.urdf.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..1f4860ce6de766ec464d3e5a161a9fb139f578b2
--- /dev/null
+++ b/ur_description/urdf/ur10_joint_limited.urdf.xacro
@@ -0,0 +1,284 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find ur_description)/urdf/ur10.transmission.xacro" />
+ <xacro:include filename="$(find ur_description)/urdf/ur10.gazebo.xacro" />
+ <xacro:include filename="$(find ur_description)/urdf/materials.urdf.xacro" />
+
+ <property name="pi" value="3.14159265" />
+
+ <!-- Inertia parameters -->
+ <property name="base_mass" value="4.0" />
+ <property name="shoulder_mass" value="3.7000" />
+ <property name="upper_arm_mass" value="8.3930" />
+ <property name="forearm_mass" value="2.2750" />
+ <property name="wrist_1_mass" value="1.2190" />
+ <property name="wrist_2_mass" value="1.2190" />
+ <property name="wrist_3_mass" value="0.1879" />
+
+ <property name="shoulder_cog" value="0.0 0.00193 -0.02561" />
+ <property name="upper_arm_cog" value="0.0 -0.024201 0.2125" />
+ <property name="forearm_cog" value="0.0 0.0265 0.11993" />
+ <property name="wrist_1_cog" value="0.0 0.110949 0.01634" />
+ <property name="wrist_2_cog" value="0.0 0.0018 0.11099" />
+ <property name="wrist_3_cog" value="0.0 0.001159 0.0" />
+
+ <!-- Kinematic model -->
+ <property name="shoulder_height" value="0.128" />
+ <property name="shoulder_offset" value="0.1704" />
+ <property name="upper_arm_length" value="0.60186" />
+ <property name="elbow_offset" value="0.12817" />
+ <property name="forearm_length" value="0.56415" />
+ <property name="wrist_1_length" value="0.11279" />
+ <property name="wrist_2_length" value="0.11279" />
+ <property name="wrist_3_length" value="0.0857" />
+
+ <property name="shoulder_radius" value="0.060" />
+ <property name="upper_arm_radius" value="0.054" />
+ <property name="elbow_radius" value="0.060" />
+ <property name="forearm_radius" value="0.040" />
+ <property name="wrist_radius" value="0.045" />
+
+ <!-- Collision model -->
+ <property name="base_collision_length" value="0.160" />
+ <property name="shoulder_collision_length" value="0.200" />
+ <property name="shoulder_collision_offset" value="0.035" />
+ <property name="elbow_collision_length" value="0.200" />
+ <property name="elbow_collision_offset" value="0.035" />
+
+
+
+ <xacro:macro name="ur10_robot" params="prefix">
+
+ <link name="${prefix}base_link" >
+ </link>
+
+ <joint name="${prefix}base_link_joint" type="fixed">
+ <parent link="${prefix}base_link" />
+ <child link = "${prefix}ur_base_link" />
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </joint>
+
+ <link name="${prefix}ur_base_link" >
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Base.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
+ <material name="UR/Blue" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Base.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 ${3.0 * pi / 4.0}" />
+ </collision>
+
+ <inertial>
+ <mass value="${base_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}shoulder_pan_joint" type="revolute">
+ <parent link="${prefix}ur_base_link" />
+ <child link = "${prefix}shoulder_link" />
+ <origin xyz="0.0 0.0 ${shoulder_height}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 0.0 1.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="330.0" velocity="2.16"/>
+ <dynamics damping="1.2" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}shoulder_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Shoulder.stl" />
+ </geometry>
+ <material name="UR/Grey" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Shoulder.stl" />
+ </geometry>
+ </collision>
+
+ <inertial>
+ <mass value="${shoulder_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin xyz="${shoulder_cog}" rpy="0.0 0.0 0.0" />
+ </inertial>
+ </link>
+
+ <joint name="${prefix}shoulder_lift_joint" type="revolute">
+ <parent link="${prefix}shoulder_link" />
+ <child link = "${prefix}upper_arm_link" />
+ <origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 ${pi / 2.0} 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="10.0" velocity="${pi}"/>
+ <dynamics damping="1.2" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}upper_arm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/UpperArm.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ <material name="UR/Blue" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/UpperArm.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
+ </collision>
+
+ <inertial>
+ <mass value="${upper_arm_mass}" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ <origin xyz="${upper_arm_cog}" rpy="0.0 0.0 0.0" />
+ </inertial>
+ </link>
+
+ <joint name="${prefix}elbow_joint" type="revolute">
+ <parent link="${prefix}upper_arm_link" />
+ <child link = "${prefix}forearm_link" />
+ <origin xyz="0.0 ${-elbow_offset} ${upper_arm_length}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="150.0" velocity="${pi}"/>
+ <dynamics damping="0.6" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}forearm_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Forearm.stl" />
+ </geometry>
+ <material name="UR/Grey" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Forearm.stl" />
+ </geometry>
+ </collision>
+
+ <inertial>
+ <mass value="${forearm_mass}" />
+ <origin xyz="${forearm_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}wrist_1_joint" type="revolute">
+ <parent link="${prefix}forearm_link" />
+ <child link = "${prefix}wrist_1_link" />
+ <origin xyz="0.0 0.0 ${forearm_length}" rpy="0.0 ${pi / 2.0} 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="${pi}"/>
+ <dynamics damping="0.6" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}wrist_1_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Wrist1.stl" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0 0.0" />
+ <material name="UR/Blue" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Wrist1.stl" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0 0.0" />
+ </collision>
+
+ <inertial>
+ <mass value="${wrist_1_mass}" />
+ <origin xyz="${wrist_1_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}wrist_2_joint" type="revolute">
+ <parent link="${prefix}wrist_1_link" />
+ <child link = "${prefix}wrist_2_link" />
+ <origin xyz="0.0 ${wrist_1_length} 0.0" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 0.0 1.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="${pi}"/>
+ <dynamics damping="0.6" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}wrist_2_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Wrist2.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ <material name="UR/Grey" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Wrist2.stl" />
+ </geometry>
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ </collision>
+
+ <inertial>
+ <mass value="${wrist_2_mass}" />
+ <origin xyz="${wrist_2_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}wrist_3_joint" type="revolute">
+ <parent link="${prefix}wrist_2_link" />
+ <child link = "${prefix}wrist_3_link" />
+ <origin xyz="0.0 0.0 ${wrist_2_length}" rpy="0.0 0.0 0.0" />
+ <axis xyz="0.0 1.0 0.0" />
+ <limit lower="${-pi}" upper="${pi}" effort="54.0" velocity="${pi}"/>
+ <dynamics damping="0.6" friction="0.0"/>
+ </joint>
+
+ <link name="${prefix}wrist_3_link">
+ <visual>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/visual/Wrist3.stl" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 0" />
+ <material name="UR/Blue" />
+ </visual>
+
+ <collision>
+ <geometry>
+ <mesh filename="package://ur_description/meshes/ur10/collision/Wrist3.stl" />
+ </geometry>
+ <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 0" />
+ </collision>
+
+ <inertial>
+ <mass value="${wrist_3_mass}" />
+ <origin xyz="${wrist_3_cog}" rpy="0.0 0.0 0.0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+ </inertial>
+ </link>
+
+ <joint name="${prefix}ee_fixed_joint" type="fixed">
+ <parent link="${prefix}wrist_3_link" />
+ <child link = "${prefix}ee_link" />
+ <origin xyz="0.0 ${wrist_3_length} 0.0" rpy="0.0 0.0 ${pi/2.0}" />
+ </joint>
+
+ <link name="${prefix}ee_link" />
+
+ <xacro:ur10_arm_transmission prefix="${prefix}" />
+ <xacro:ur10_arm_gazebo prefix="${prefix}" />
+
+ </xacro:macro>
+</robot>
diff --git a/ur_kinematics/CMakeLists.txt b/ur_kinematics/CMakeLists.txt
index d7d5f2adf97086f73b7042a9da49e4f3edadbfe3..e6e5b4097b4b7258971a5f7ad05f2932654111d6 100644
--- a/ur_kinematics/CMakeLists.txt
+++ b/ur_kinematics/CMakeLists.txt
@@ -4,11 +4,10 @@ project(ur_kinematics)
## 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)
+find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs moveit_core moveit_ros_planning pluginlib tf_conversions)
## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
+find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
@@ -49,7 +48,9 @@ find_package(catkin REQUIRED)
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
- LIBRARIES ur10_kin ur5_kin
+ LIBRARIES ur10_kin ur5_kin ur10_moveit_plugin ur5_moveit_plugin
+ CATKIN_DEPENDS pluginlib
+ DEPENDS moveit_core
# CATKIN_DEPENDS other_catkin_pkg
# DEPENDS system_lib
)
@@ -60,8 +61,12 @@ catkin_package(
## Specify additional locations of header files
## Your package locations should be listed before other locations
+include_directories(SYSTEM ${Boost_INCLUDE_DIR})
include_directories(include ${catkin_INCLUDE_DIRS})
+link_directories(${Boost_LIBRARY_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
## Declare a cpp library
# add_library(ur_kinematics
# src/${PROJECT_NAME}/ur_kinematics.cpp
@@ -72,6 +77,20 @@ set_target_properties(ur10_kin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
add_library(ur5_kin src/ur_kin.cpp)
set_target_properties(ur5_kin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+add_library(ur10_moveit_plugin src/ur_moveit_plugin.cpp)
+set_target_properties(ur10_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR10_PARAMS")
+target_link_libraries(ur10_moveit_plugin
+ ${catkin_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ur10_kin)
+
+add_library(ur5_moveit_plugin src/ur_moveit_plugin.cpp)
+set_target_properties(ur5_moveit_plugin PROPERTIES COMPILE_DEFINITIONS "UR5_PARAMS")
+target_link_libraries(ur5_moveit_plugin
+ ${catkin_LIBRARIES}
+ ${Boost_LIBRARIES}
+ ur5_kin)
+
## Declare a cpp executable
# add_executable(ur_kinematics_node src/ur_kinematics_node.cpp)
@@ -124,11 +143,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-# # myfile1
-# # myfile2
-# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+install(FILES
+ ur_moveit_plugins.xml
+ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
#############
## Testing ##
diff --git a/ur_kinematics/include/ur_kinematics/ur_kin.h b/ur_kinematics/include/ur_kinematics/ur_kin.h
index 30b677235030e0d2f36496497159804d0ad73b30..ee5a0465f033f2b2eb526832b1e548375131ae92 100644
--- a/ur_kinematics/include/ur_kinematics/ur_kin.h
+++ b/ur_kinematics/include/ur_kinematics/ur_kin.h
@@ -86,6 +86,11 @@ namespace ur_kinematics {
// @param T The 4x4 end effector pose in row-major ordering
void forward(const double* q, double* T);
+ // @param q The 6 joint values
+ // @param Ti The 4x4 link i pose in row-major ordering. If NULL, nothing is stored.
+ void forward_all(const double* q, double* T1, double* T2, double* T3,
+ double* T4, double* T5, double* T6);
+
// @param T The 4x4 end effector pose in row-major ordering
// @param q_sols An 8x6 array of doubles returned, all angles should be in [0,2*PI)
// @param q6_des An optional parameter which designates what the q6 value should take
diff --git a/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
new file mode 100644
index 0000000000000000000000000000000000000000..2fee591899096c9b7a2f809226e286ac41f2898f
--- /dev/null
+++ b/ur_kinematics/include/ur_kinematics/ur_moveit_plugin.h
@@ -0,0 +1,283 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2014, Georgia Tech
+* 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 Georgia Tech 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 OWNER 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.
+*********************************************************************/
+
+/* Author: Kelsey Hawkins */
+
+/* Based on orignal source from Willow Garage. License copied below */
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Willow Garage, Inc.
+* 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 Willow Garage 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 OWNER 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.
+*********************************************************************/
+
+/* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
+
+#ifndef UR_KINEMATICS_PLUGIN_
+#define UR_KINEMATICS_PLUGIN_
+
+// ROS
+#include <ros/ros.h>
+#include <random_numbers/random_numbers.h>
+
+// System
+#include <boost/shared_ptr.hpp>
+
+// ROS msgs
+#include <geometry_msgs/PoseStamped.h>
+#include <moveit_msgs/GetPositionFK.h>
+#include <moveit_msgs/GetPositionIK.h>
+#include <moveit_msgs/GetKinematicSolverInfo.h>
+#include <moveit_msgs/MoveItErrorCodes.h>
+
+// KDL
+#include <kdl/jntarray.hpp>
+#include <kdl/chainiksolvervel_pinv.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainfksolverpos_recursive.hpp>
+#include <moveit/kdl_kinematics_plugin/chainiksolver_pos_nr_jl_mimic.hpp>
+#include <moveit/kdl_kinematics_plugin/chainiksolver_vel_pinv_mimic.hpp>
+#include <moveit/kdl_kinematics_plugin/joint_mimic.hpp>
+
+// MoveIt!
+#include <moveit/kinematics_base/kinematics_base.h>
+#include <moveit/robot_model/robot_model.h>
+#include <moveit/robot_state/robot_state.h>
+
+namespace ur_kinematics
+{
+/**
+* @brief Specific implementation of kinematics using KDL. This version can be used with any robot.
+*/
+ class URKinematicsPlugin : public kinematics::KinematicsBase
+ {
+ public:
+
+ /**
+* @brief Default constructor
+*/
+ URKinematicsPlugin();
+
+ virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ const std::vector<double> &consistency_limits,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ const std::vector<double> &consistency_limits,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool getPositionFK(const std::vector<std::string> &link_names,
+ const std::vector<double> &joint_angles,
+ std::vector<geometry_msgs::Pose> &poses) const;
+
+ virtual bool initialize(const std::string &robot_description,
+ const std::string &group_name,
+ const std::string &base_name,
+ const std::string &tip_name,
+ double search_discretization);
+
+ /**
+* @brief Return all the joint names in the order they are used internally
+*/
+ const std::vector<std::string>& getJointNames() const;
+
+ /**
+* @brief Return all the link names in the order they are represented internally
+*/
+ const std::vector<std::string>& getLinkNames() const;
+
+ protected:
+
+ /**
+* @brief Given a desired pose of the end-effector, search for the joint angles required to reach it.
+* This particular method is intended for "searching" for a solutions by stepping through the redundancy
+* (or other numerical routines).
+* @param ik_pose the desired pose of the link
+* @param ik_seed_state an initial guess solution for the inverse kinematics
+* @param timeout The amount of time (in seconds) available to the solver
+* @param solution the solution vector
+* @param solution_callback A callback solution for the IK solution
+* @param error_code an error code that encodes the reason for failure or success
+* @param check_consistency Set to true if consistency check needs to be performed
+* @param redundancy The index of the redundant joint
+* @param consistency_limit The returned solutuion will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+* @return True if a valid solution was found, false otherwise
+*/
+ bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const std::vector<double> &consistency_limits,
+ const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
+
+ virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
+
+ private:
+
+ bool timedOut(const ros::WallTime &start_time, double duration) const;
+
+
+ /** @brief Check whether the solution lies within the consistency limit of the seed state
+* @param seed_state Seed state
+* @param redundancy Index of the redundant joint within the chain
+* @param consistency_limit The returned state for redundant joint should be in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+* @param solution solution configuration
+* @return true if check succeeds
+*/
+ bool checkConsistency(const KDL::JntArray& seed_state,
+ const std::vector<double> &consistency_limit,
+ const KDL::JntArray& solution) const;
+
+ int getJointIndex(const std::string &name) const;
+
+ int getKDLSegmentIndex(const std::string &name) const;
+
+ void getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const;
+
+ /** @brief Get a random configuration within joint limits close to the seed state
+* @param seed_state Seed state
+* @param redundancy Index of the redundant joint within the chain
+* @param consistency_limit The returned state will contain a value for the redundant joint in the range [seed_state(redundancy_limit)-consistency_limit,seed_state(redundancy_limit)+consistency_limit]
+* @param jnt_array Returned random configuration
+*/
+ void getRandomConfiguration(const KDL::JntArray& seed_state,
+ const std::vector<double> &consistency_limits,
+ KDL::JntArray &jnt_array,
+ bool lock_redundancy) const;
+
+ bool isRedundantJoint(unsigned int index) const;
+
+ bool active_; /** Internal variable that indicates whether solvers are configured and ready */
+
+ moveit_msgs::KinematicSolverInfo ik_chain_info_; /** Stores information for the inverse kinematics solver */
+
+ moveit_msgs::KinematicSolverInfo fk_chain_info_; /** Store information for the forward kinematics solver */
+
+ KDL::Chain kdl_chain_;
+
+ unsigned int dimension_; /** Dimension of the group */
+
+ KDL::JntArray joint_min_, joint_max_; /** Joint limits */
+
+ mutable random_numbers::RandomNumberGenerator random_number_generator_;
+
+ robot_model::RobotModelPtr robot_model_;
+
+ robot_state::RobotStatePtr state_, state_2_;
+
+ int num_possible_redundant_joints_;
+ std::vector<unsigned int> redundant_joints_map_index_;
+
+ // Storage required for when the set of redundant joints is reset
+ bool position_ik_; //whether this solver is only being used for position ik
+ robot_model::JointModelGroup* joint_model_group_;
+ double max_solver_iterations_;
+ double epsilon_;
+ std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints_;
+
+ std::vector<double> ik_weights_;
+ std::vector<std::string> ur_joint_names_;
+ std::vector<std::string> ur_link_names_;
+ int ur_joint_inds_start_;
+ std::string arm_prefix_;
+
+ // kinematic chains representing the chain from the group base link to the
+ // UR base link, and the UR tip link to the group tip link
+ KDL::Chain kdl_base_chain_;
+ KDL::Chain kdl_tip_chain_;
+
+ };
+}
+
+#endif
diff --git a/ur_kinematics/package.xml b/ur_kinematics/package.xml
index 497106dc7f787c89b614d9a9a421e2180dc7ab0d..2891b86c05e453212f3eeba09c7aa561f02cdb24 100644
--- a/ur_kinematics/package.xml
+++ b/ur_kinematics/package.xml
@@ -4,6 +4,7 @@
<version>1.0.2</version>
<description>
Provides forward and inverse kinematics for Universal robot designs.
+ See http://hdl.handle.net/1853/50782 for details.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
@@ -42,15 +43,26 @@
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
+
<buildtool_depend>catkin</buildtool_depend>
+ <build_depend>moveit_core</build_depend>
+ <build_depend>moveit_ros_planning</build_depend>
+ <build_depend>roscpp</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+ <build_depend>pluginlib</build_depend>
+ <build_depend>tf_conversions</build_depend>
- <!-- The export tag contains other, unspecified, tags -->
- <export>
- <!-- You can specify that this package is a metapackage here: -->
- <!-- <metapackage/> -->
+ <run_depend>moveit_core</run_depend>
+ <run_depend>moveit_ros_planning</run_depend>
+ <run_depend>roscpp</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>pluginlib</run_depend>
+ <run_depend>tf_conversions</run_depend>
- <!-- Other tools can request additional information be placed here -->
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <moveit_core plugin="${prefix}/ur_moveit_plugins.xml"/>
</export>
</package>
diff --git a/ur_kinematics/src/ur_kin.cpp b/ur_kinematics/src/ur_kin.cpp
index 57144541808a2b1fd4a07d56dbaf942f8515692a..8c92ea3a13341b8c6f208e6fe5826563e5761adc 100644
--- a/ur_kinematics/src/ur_kin.cpp
+++ b/ur_kinematics/src/ur_kin.cpp
@@ -34,6 +34,132 @@ namespace ur_kinematics {
*T = 0.0; T++; *T = 0.0; T++; *T = 0.0; T++; *T = 1.0;
}
+ void forward_all(const double* q, double* T1, double* T2, double* T3,
+ double* T4, double* T5, double* T6) {
+ double s1 = sin(*q), c1 = cos(*q); q++; // q1
+ double q23 = *q, q234 = *q, s2 = sin(*q), c2 = cos(*q); q++; // q2
+ double s3 = sin(*q), c3 = cos(*q); q23 += *q; q234 += *q; q++; // q3
+ q234 += *q; q++; // q4
+ double s5 = sin(*q), c5 = cos(*q); q++; // q5
+ double s6 = sin(*q), c6 = cos(*q); // q6
+ double s23 = sin(q23), c23 = cos(q23);
+ double s234 = sin(q234), c234 = cos(q234);
+
+ if(T1 != NULL) {
+ *T1 = c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = s1; T1++;
+ *T1 = 0; T1++;
+ *T1 = -c1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+ *T1 = 0; T1++;
+ *T1 =d1; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 0; T1++;
+ *T1 = 1; T1++;
+ }
+
+ if(T2 != NULL) {
+ *T2 = c1*c2; T2++;
+ *T2 = -c1*s2; T2++;
+ *T2 = s1; T2++;
+ *T2 =a2*c1*c2; T2++;
+ *T2 = c2*s1; T2++;
+ *T2 = -s1*s2; T2++;
+ *T2 = -c1; T2++;
+ *T2 =a2*c2*s1; T2++;
+ *T2 = s2; T2++;
+ *T2 = c2; T2++;
+ *T2 = 0; T2++;
+ *T2 = d1 + a2*s2; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 0; T2++;
+ *T2 = 1; T2++;
+ }
+
+ if(T3 != NULL) {
+ *T3 = c23*c1; T3++;
+ *T3 = -s23*c1; T3++;
+ *T3 = s1; T3++;
+ *T3 =c1*(a3*c23 + a2*c2); T3++;
+ *T3 = c23*s1; T3++;
+ *T3 = -s23*s1; T3++;
+ *T3 = -c1; T3++;
+ *T3 =s1*(a3*c23 + a2*c2); T3++;
+ *T3 = s23; T3++;
+ *T3 = c23; T3++;
+ *T3 = 0; T3++;
+ *T3 = d1 + a3*s23 + a2*s2; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 0; T3++;
+ *T3 = 1; T3++;
+ }
+
+ if(T4 != NULL) {
+ *T4 = c234*c1; T4++;
+ *T4 = s1; T4++;
+ *T4 = s234*c1; T4++;
+ *T4 =c1*(a3*c23 + a2*c2) + d4*s1; T4++;
+ *T4 = c234*s1; T4++;
+ *T4 = -c1; T4++;
+ *T4 = s234*s1; T4++;
+ *T4 =s1*(a3*c23 + a2*c2) - d4*c1; T4++;
+ *T4 = s234; T4++;
+ *T4 = 0; T4++;
+ *T4 = -c234; T4++;
+ *T4 = d1 + a3*s23 + a2*s2; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 0; T4++;
+ *T4 = 1; T4++;
+ }
+
+ if(T5 != NULL) {
+ *T5 = s1*s5 + c234*c1*c5; T5++;
+ *T5 = -s234*c1; T5++;
+ *T5 = c5*s1 - c234*c1*s5; T5++;
+ *T5 =c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T5++;
+ *T5 = c234*c5*s1 - c1*s5; T5++;
+ *T5 = -s234*s1; T5++;
+ *T5 = - c1*c5 - c234*s1*s5; T5++;
+ *T5 =s1*(a3*c23 + a2*c2) - d4*c1 + d5*s234*s1; T5++;
+ *T5 = s234*c5; T5++;
+ *T5 = c234; T5++;
+ *T5 = -s234*s5; T5++;
+ *T5 = d1 + a3*s23 + a2*s2 - d5*c234; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 0; T5++;
+ *T5 = 1; T5++;
+ }
+
+ if(T6 != NULL) {
+ *T6 = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6; T6++;
+ *T6 = - s6*(s1*s5 + c234*c1*c5) - s234*c1*c6; T6++;
+ *T6 = c5*s1 - c234*c1*s5; T6++;
+ *T6 =d6*(c5*s1 - c234*c1*s5) + c1*(a3*c23 + a2*c2) + d4*s1 + d5*s234*c1; T6++;
+ *T6 = - c6*(c1*s5 - c234*c5*s1) - s234*s1*s6; T6++;
+ *T6 = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1; T6++;
+ *T6 = - c1*c5 - c234*s1*s5; T6++;
+ *T6 =s1*(a3*c23 + a2*c2) - d4*c1 - d6*(c1*c5 + c234*s1*s5) + d5*s234*s1; T6++;
+ *T6 = c234*s6 + s234*c5*c6; T6++;
+ *T6 = c234*c6 - s234*c5*s6; T6++;
+ *T6 = -s234*s5; T6++;
+ *T6 = d1 + a3*s23 + a2*s2 - d5*c234 - d6*s234*s5; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 0; T6++;
+ *T6 = 1; T6++;
+ }
+ }
+
int inverse(const double* T, double* q_sols, double q6_des) {
int num_sols = 0;
double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
diff --git a/ur_kinematics/src/ur_moveit_plugin.cpp b/ur_kinematics/src/ur_moveit_plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56ca34fb6a552cd302cbb91208b4d0a01e4230a4
--- /dev/null
+++ b/ur_kinematics/src/ur_moveit_plugin.cpp
@@ -0,0 +1,792 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2014, Georgia Tech
+* 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 Georgia Tech 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 OWNER 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.
+*********************************************************************/
+
+/* Author: Kelsey Hawkins */
+
+/* Based on orignal source from Willow Garage. License copied below */
+
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2012, Willow Garage, Inc.
+* 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 Willow Garage 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 OWNER 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.
+*********************************************************************/
+
+/* Author: Sachin Chitta, David Lu!!, Ugo Cupcic */
+
+#include <class_loader/class_loader.h>
+
+//#include <tf/transform_datatypes.h>
+#include <tf_conversions/tf_kdl.h>
+#include <kdl_parser/kdl_parser.hpp>
+
+// URDF, SRDF
+#include <urdf_model/model.h>
+#include <srdfdom/model.h>
+
+#include <moveit/rdf_loader/rdf_loader.h>
+
+// UR kin
+#include <ur_kinematics/ur_moveit_plugin.h>
+#include <ur_kinematics/ur_kin.h>
+
+//register KDLKinematics as a KinematicsBase implementation
+CLASS_LOADER_REGISTER_CLASS(ur_kinematics::URKinematicsPlugin, kinematics::KinematicsBase)
+
+namespace ur_kinematics
+{
+
+ URKinematicsPlugin::URKinematicsPlugin():active_(false) {}
+
+void URKinematicsPlugin::getRandomConfiguration(KDL::JntArray &jnt_array, bool lock_redundancy) const
+{
+ std::vector<double> jnt_array_vector(dimension_, 0.0);
+ state_->setToRandomPositions(joint_model_group_);
+ state_->copyJointGroupPositions(joint_model_group_, &jnt_array_vector[0]);
+ for (std::size_t i = 0; i < dimension_; ++i)
+ {
+ if (lock_redundancy)
+ if (isRedundantJoint(i))
+ continue;
+ jnt_array(i) = jnt_array_vector[i];
+ }
+}
+
+bool URKinematicsPlugin::isRedundantJoint(unsigned int index) const
+{
+ for (std::size_t j=0; j < redundant_joint_indices_.size(); ++j)
+ if (redundant_joint_indices_[j] == index)
+ return true;
+ return false;
+}
+
+void URKinematicsPlugin::getRandomConfiguration(const KDL::JntArray &seed_state,
+ const std::vector<double> &consistency_limits,
+ KDL::JntArray &jnt_array,
+ bool lock_redundancy) const
+{
+ std::vector<double> values(dimension_, 0.0);
+ std::vector<double> near(dimension_, 0.0);
+ for (std::size_t i = 0 ; i < dimension_; ++i)
+ near[i] = seed_state(i);
+
+ // Need to resize the consistency limits to remove mimic joints
+ std::vector<double> consistency_limits_mimic;
+ for(std::size_t i = 0; i < dimension_; ++i)
+ {
+ if(!mimic_joints_[i].active)
+ continue;
+ consistency_limits_mimic.push_back(consistency_limits[i]);
+ }
+
+ joint_model_group_->getVariableRandomPositionsNearBy(state_->getRandomNumberGenerator(), values, near, consistency_limits_mimic);
+
+ for (std::size_t i = 0; i < dimension_; ++i)
+ {
+ bool skip = false;
+ if (lock_redundancy)
+ for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
+ if (redundant_joint_indices_[j] == i)
+ {
+ skip = true;
+ break;
+ }
+ if (skip)
+ continue;
+ jnt_array(i) = values[i];
+ }
+}
+
+bool URKinematicsPlugin::checkConsistency(const KDL::JntArray& seed_state,
+ const std::vector<double> &consistency_limits,
+ const KDL::JntArray& solution) const
+{
+ for (std::size_t i = 0; i < dimension_; ++i)
+ if (fabs(seed_state(i) - solution(i)) > consistency_limits[i])
+ return false;
+ return true;
+}
+
+bool URKinematicsPlugin::initialize(const std::string &robot_description,
+ const std::string& group_name,
+ const std::string& base_frame,
+ const std::string& tip_frame,
+ double search_discretization)
+{
+ setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
+
+ ros::NodeHandle private_handle("~");
+ rdf_loader::RDFLoader rdf_loader(robot_description_);
+ const boost::shared_ptr<srdf::Model> &srdf = rdf_loader.getSRDF();
+ const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
+
+ if (!urdf_model || !srdf)
+ {
+ ROS_ERROR_NAMED("kdl","URDF and SRDF must be loaded for KDL kinematics solver to work.");
+ return false;
+ }
+
+ robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
+
+ robot_model::JointModelGroup* joint_model_group = robot_model_->getJointModelGroup(group_name);
+ if (!joint_model_group)
+ return false;
+
+ if(!joint_model_group->isChain())
+ {
+ ROS_ERROR_NAMED("kdl","Group '%s' is not a chain", group_name.c_str());
+ return false;
+ }
+ if(!joint_model_group->isSingleDOFJoints())
+ {
+ ROS_ERROR_NAMED("kdl","Group '%s' includes joints that have more than 1 DOF", group_name.c_str());
+ return false;
+ }
+
+ KDL::Tree kdl_tree;
+
+ if (!kdl_parser::treeFromUrdfModel(*urdf_model, kdl_tree))
+ {
+ ROS_ERROR_NAMED("kdl","Could not initialize tree object");
+ return false;
+ }
+ if (!kdl_tree.getChain(base_frame_, getTipFrame(), kdl_chain_))
+ {
+ ROS_ERROR_NAMED("kdl","Could not initialize chain object");
+ return false;
+ }
+
+ dimension_ = joint_model_group->getActiveJointModels().size() + joint_model_group->getMimicJointModels().size();
+ for (std::size_t i=0; i < joint_model_group->getJointModels().size(); ++i)
+ {
+ if(joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::REVOLUTE || joint_model_group->getJointModels()[i]->getType() == moveit::core::JointModel::PRISMATIC)
+ {
+ ik_chain_info_.joint_names.push_back(joint_model_group->getJointModelNames()[i]);
+ const std::vector<moveit_msgs::JointLimits> &jvec = joint_model_group->getJointModels()[i]->getVariableBoundsMsg();
+ ik_chain_info_.limits.insert(ik_chain_info_.limits.end(), jvec.begin(), jvec.end());
+ }
+ }
+
+ fk_chain_info_.joint_names = ik_chain_info_.joint_names;
+ fk_chain_info_.limits = ik_chain_info_.limits;
+
+ if(!joint_model_group->hasLinkModel(getTipFrame()))
+ {
+ ROS_ERROR_NAMED("kdl","Could not find tip name in joint group '%s'", group_name.c_str());
+ return false;
+ }
+ ik_chain_info_.link_names.push_back(getTipFrame());
+ fk_chain_info_.link_names = joint_model_group->getLinkModelNames();
+
+ joint_min_.resize(ik_chain_info_.limits.size());
+ joint_max_.resize(ik_chain_info_.limits.size());
+
+ for(unsigned int i=0; i < ik_chain_info_.limits.size(); i++)
+ {
+ joint_min_(i) = ik_chain_info_.limits[i].min_position;
+ joint_max_(i) = ik_chain_info_.limits[i].max_position;
+ }
+
+ // Get Solver Parameters
+ int max_solver_iterations;
+ double epsilon;
+ bool position_ik;
+
+ private_handle.param("max_solver_iterations", max_solver_iterations, 500);
+ private_handle.param("epsilon", epsilon, 1e-5);
+ private_handle.param(group_name+"/position_only_ik", position_ik, false);
+ ROS_DEBUG_NAMED("kdl","Looking in private handle: %s for param name: %s",
+ private_handle.getNamespace().c_str(),
+ (group_name+"/position_only_ik").c_str());
+
+ if(position_ik)
+ ROS_INFO_NAMED("kdl","Using position only ik");
+
+ num_possible_redundant_joints_ = kdl_chain_.getNrOfJoints() - joint_model_group->getMimicJointModels().size() - (position_ik? 3:6);
+
+ // Check for mimic joints
+ bool has_mimic_joints = joint_model_group->getMimicJointModels().size() > 0;
+ std::vector<unsigned int> redundant_joints_map_index;
+
+ std::vector<kdl_kinematics_plugin::JointMimic> mimic_joints;
+ unsigned int joint_counter = 0;
+ for (std::size_t i = 0; i < kdl_chain_.getNrOfSegments(); ++i)
+ {
+ const robot_model::JointModel *jm = robot_model_->getJointModel(kdl_chain_.segments[i].getJoint().getName());
+
+ //first check whether it belongs to the set of active joints in the group
+ if (jm->getMimic() == NULL && jm->getVariableCount() > 0)
+ {
+ kdl_kinematics_plugin::JointMimic mimic_joint;
+ mimic_joint.reset(joint_counter);
+ mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
+ mimic_joint.active = true;
+ mimic_joints.push_back(mimic_joint);
+ ++joint_counter;
+ continue;
+ }
+ if (joint_model_group->hasJointModel(jm->getName()))
+ {
+ if (jm->getMimic() && joint_model_group->hasJointModel(jm->getMimic()->getName()))
+ {
+ kdl_kinematics_plugin::JointMimic mimic_joint;
+ mimic_joint.reset(joint_counter);
+ mimic_joint.joint_name = kdl_chain_.segments[i].getJoint().getName();
+ mimic_joint.offset = jm->getMimicOffset();
+ mimic_joint.multiplier = jm->getMimicFactor();
+ mimic_joints.push_back(mimic_joint);
+ continue;
+ }
+ }
+ }
+ for (std::size_t i = 0; i < mimic_joints.size(); ++i)
+ {
+ if(!mimic_joints[i].active)
+ {
+ const robot_model::JointModel* joint_model = joint_model_group->getJointModel(mimic_joints[i].joint_name)->getMimic();
+ for(std::size_t j=0; j < mimic_joints.size(); ++j)
+ {
+ if(mimic_joints[j].joint_name == joint_model->getName())
+ {
+ mimic_joints[i].map_index = mimic_joints[j].map_index;
+ }
+ }
+ }
+ }
+ mimic_joints_ = mimic_joints;
+
+ // Setup the joint state groups that we need
+ state_.reset(new robot_state::RobotState(robot_model_));
+ state_2_.reset(new robot_state::RobotState(robot_model_));
+
+ // Store things for when the set of redundant joints may change
+ position_ik_ = position_ik;
+ joint_model_group_ = joint_model_group;
+ max_solver_iterations_ = max_solver_iterations;
+ epsilon_ = epsilon;
+
+ private_handle.param<std::string>("arm_prefix", arm_prefix_, "");
+
+ ur_joint_names_.push_back(arm_prefix_ + "shoulder_pan_joint");
+ ur_joint_names_.push_back(arm_prefix_ + "shoulder_lift_joint");
+ ur_joint_names_.push_back(arm_prefix_ + "elbow_joint");
+ ur_joint_names_.push_back(arm_prefix_ + "wrist_1_joint");
+ ur_joint_names_.push_back(arm_prefix_ + "wrist_2_joint");
+ ur_joint_names_.push_back(arm_prefix_ + "wrist_3_joint");
+
+ ur_link_names_.push_back(arm_prefix_ + "base_link"); // 0
+ ur_link_names_.push_back(arm_prefix_ + "ur_base_link"); // 1
+ ur_link_names_.push_back(arm_prefix_ + "shoulder_link"); // 2
+ ur_link_names_.push_back(arm_prefix_ + "upper_arm_link"); // 3
+ ur_link_names_.push_back(arm_prefix_ + "forearm_link"); // 4
+ ur_link_names_.push_back(arm_prefix_ + "wrist_1_link"); // 5
+ ur_link_names_.push_back(arm_prefix_ + "wrist_2_link"); // 6
+ ur_link_names_.push_back(arm_prefix_ + "wrist_3_link"); // 7
+ ur_link_names_.push_back(arm_prefix_ + "ee_link"); // 8
+
+ ur_joint_inds_start_ = getJointIndex(ur_joint_names_[0]);
+
+ // check to make sure the serial chain is properly defined in the model
+ int cur_ur_joint_ind, last_ur_joint_ind = ur_joint_inds_start_;
+ for(int i=1; i<6; i++) {
+ cur_ur_joint_ind = getJointIndex(ur_joint_names_[i]);
+ if(cur_ur_joint_ind < 0) {
+ ROS_ERROR_NAMED("kdl",
+ "Kin chain provided in model doesn't contain standard UR joint '%s'.",
+ ur_joint_names_[i].c_str());
+ return false;
+ }
+ if(cur_ur_joint_ind != last_ur_joint_ind + 1) {
+ ROS_ERROR_NAMED("kdl",
+ "Kin chain provided in model doesn't have proper serial joint order: '%s'.",
+ ur_joint_names_[i].c_str());
+ return false;
+ }
+ last_ur_joint_ind = cur_ur_joint_ind;
+ }
+ // if successful, the kinematic chain includes a serial chain of the UR joints
+
+ kdl_tree.getChain(getBaseFrame(), ur_link_names_.front(), kdl_base_chain_);
+ kdl_tree.getChain(ur_link_names_.back(), getTipFrame(), kdl_tip_chain_);
+
+ // weights for redundant solution selection
+ ik_weights_.resize(6);
+ if(private_handle.hasParam("ik_weights")) {
+ private_handle.getParam("ik_weights", ik_weights_);
+ } else {
+ ik_weights_[0] = 1.0;
+ ik_weights_[1] = 1.0;
+ ik_weights_[2] = 0.1;
+ ik_weights_[3] = 0.1;
+ ik_weights_[4] = 0.3;
+ ik_weights_[5] = 0.3;
+ }
+
+ active_ = true;
+ ROS_DEBUG_NAMED("kdl","KDL solver initialized");
+ return true;
+}
+
+bool URKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int> &redundant_joints)
+{
+ if(num_possible_redundant_joints_ < 0)
+ {
+ ROS_ERROR_NAMED("kdl","This group cannot have redundant joints");
+ return false;
+ }
+ if(redundant_joints.size() > num_possible_redundant_joints_)
+ {
+ ROS_ERROR_NAMED("kdl","This group can only have %d redundant joints", num_possible_redundant_joints_);
+ return false;
+ }
+ std::vector<unsigned int> redundant_joints_map_index;
+ unsigned int counter = 0;
+ for(std::size_t i=0; i < dimension_; ++i)
+ {
+ bool is_redundant_joint = false;
+ for(std::size_t j=0; j < redundant_joints.size(); ++j)
+ {
+ if(i == redundant_joints[j])
+ {
+ is_redundant_joint = true;
+counter++;
+ break;
+ }
+ }
+ if(!is_redundant_joint)
+ {
+ // check for mimic
+ if(mimic_joints_[i].active)
+ {
+redundant_joints_map_index.push_back(counter);
+counter++;
+ }
+ }
+ }
+ for(std::size_t i=0; i < redundant_joints_map_index.size(); ++i)
+ ROS_DEBUG_NAMED("kdl","Redundant joint map index: %d %d", (int) i, (int) redundant_joints_map_index[i]);
+
+ redundant_joints_map_index_ = redundant_joints_map_index;
+ redundant_joint_indices_ = redundant_joints;
+ return true;
+}
+
+int URKinematicsPlugin::getJointIndex(const std::string &name) const
+{
+ for (unsigned int i=0; i < ik_chain_info_.joint_names.size(); i++) {
+ if (ik_chain_info_.joint_names[i] == name)
+ return i;
+ }
+ return -1;
+}
+
+int URKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
+{
+ int i=0;
+ while (i < (int)kdl_chain_.getNrOfSegments()) {
+ if (kdl_chain_.getSegment(i).getName() == name) {
+ return i+1;
+ }
+ i++;
+ }
+ return -1;
+}
+
+bool URKinematicsPlugin::timedOut(const ros::WallTime &start_time, double duration) const
+{
+ return ((ros::WallTime::now()-start_time).toSec() >= duration);
+}
+
+bool URKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ const IKCallbackFn solution_callback = 0;
+ std::vector<double> consistency_limits;
+
+ return searchPositionIK(ik_pose,
+ ik_seed_state,
+ default_timeout_,
+ solution,
+ solution_callback,
+ error_code,
+ consistency_limits,
+ options);
+}
+
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ const IKCallbackFn solution_callback = 0;
+ std::vector<double> consistency_limits;
+
+ return searchPositionIK(ik_pose,
+ ik_seed_state,
+ timeout,
+ solution,
+ solution_callback,
+ error_code,
+ consistency_limits,
+ options);
+}
+
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ const std::vector<double> &consistency_limits,
+ std::vector<double> &solution,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ const IKCallbackFn solution_callback = 0;
+ return searchPositionIK(ik_pose,
+ ik_seed_state,
+ timeout,
+ solution,
+ solution_callback,
+ error_code,
+ consistency_limits,
+ options);
+}
+
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ std::vector<double> consistency_limits;
+ return searchPositionIK(ik_pose,
+ ik_seed_state,
+ timeout,
+ solution,
+ solution_callback,
+ error_code,
+ consistency_limits,
+ options);
+}
+
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ const std::vector<double> &consistency_limits,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ return searchPositionIK(ik_pose,
+ ik_seed_state,
+ timeout,
+ solution,
+ solution_callback,
+ error_code,
+ consistency_limits,
+ options);
+}
+
+typedef std::pair<int, double> idx_double;
+bool comparator(const idx_double& l, const idx_double& r)
+{ return l.second < r.second; }
+
+bool URKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
+ const std::vector<double> &ik_seed_state,
+ double timeout,
+ std::vector<double> &solution,
+ const IKCallbackFn &solution_callback,
+ moveit_msgs::MoveItErrorCodes &error_code,
+ const std::vector<double> &consistency_limits,
+ const kinematics::KinematicsQueryOptions &options) const
+{
+ ros::WallTime n1 = ros::WallTime::now();
+ if(!active_) {
+ ROS_ERROR_NAMED("kdl","kinematics not active");
+ error_code.val = error_code.NO_IK_SOLUTION;
+ return false;
+ }
+
+ if(ik_seed_state.size() != dimension_) {
+ ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
+ error_code.val = error_code.NO_IK_SOLUTION;
+ return false;
+ }
+
+ if(!consistency_limits.empty() && consistency_limits.size() != dimension_) {
+ ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
+ error_code.val = error_code.NO_IK_SOLUTION;
+ return false;
+ }
+
+ KDL::JntArray jnt_seed_state(dimension_);
+ for(int i=0; i<dimension_; i++)
+ jnt_seed_state(i) = ik_seed_state[i];
+
+ solution.resize(dimension_);
+
+ KDL::ChainFkSolverPos_recursive fk_solver_base(kdl_base_chain_);
+ KDL::ChainFkSolverPos_recursive fk_solver_tip(kdl_tip_chain_);
+
+ KDL::JntArray jnt_pos_test(jnt_seed_state);
+ KDL::JntArray jnt_pos_base(ur_joint_inds_start_);
+ KDL::JntArray jnt_pos_tip(dimension_ - 6 - ur_joint_inds_start_);
+ KDL::Frame pose_base, pose_tip;
+
+ KDL::Frame kdl_ik_pose;
+ KDL::Frame kdl_ik_pose_ur_chain;
+ double homo_ik_pose[4][4];
+ double q_ik_sols[8][6]; // maximum of 8 IK solutions
+ uint16_t num_sols;
+
+ while(1) {
+ if(timedOut(n1, timeout)) {
+ ROS_DEBUG_NAMED("kdl","IK timed out");
+ error_code.val = error_code.TIMED_OUT;
+ return false;
+ }
+
+ /////////////////////////////////////////////////////////////////////////////
+ // find transformation from robot base to UR base and UR tip to robot tip
+ for(uint32_t i=0; i<jnt_pos_base.rows(); i++)
+ jnt_pos_base(i) = jnt_pos_test(i);
+ for(uint32_t i=0; i<jnt_pos_tip.rows(); i++)
+ jnt_pos_tip(i) = jnt_pos_test(i + ur_joint_inds_start_ + 6);
+ for(uint32_t i=0; i<jnt_seed_state.rows(); i++)
+ solution[i] = jnt_pos_test(i);
+
+ if(fk_solver_base.JntToCart(jnt_pos_base, pose_base) < 0) {
+ ROS_ERROR_NAMED("kdl", "Could not compute FK for base chain");
+ return false;
+ }
+
+ if(fk_solver_tip.JntToCart(jnt_pos_tip, pose_tip) < 0) {
+ ROS_ERROR_NAMED("kdl", "Could not compute FK for tip chain");
+ return false;
+ }
+ /////////////////////////////////////////////////////////////////////////////
+
+ /////////////////////////////////////////////////////////////////////////////
+ // Convert into query for analytic solver
+ tf::poseMsgToKDL(ik_pose, kdl_ik_pose);
+ kdl_ik_pose_ur_chain = pose_base.Inverse() * kdl_ik_pose * pose_tip.Inverse();
+
+ kdl_ik_pose_ur_chain.Make4x4((double*) homo_ik_pose);
+#if KDL_OLD_BUG_FIX
+ // in older versions of KDL, setting this flag might be necessary
+ for(int i=0; i<3; i++) homo_ik_pose[i][3] *= 1000; // strange KDL fix
+#endif
+ /////////////////////////////////////////////////////////////////////////////
+
+ // Do the analytic IK
+ num_sols = inverse((double*) homo_ik_pose, (double*) q_ik_sols,
+ jnt_pos_test(ur_joint_inds_start_+5));
+
+ // use weighted absolute deviations to determine the solution closest the seed state
+ std::vector<idx_double> weighted_diffs;
+ for(uint16_t i=0; i<num_sols; i++) {
+ double cur_weighted_diff = 0;
+ for(uint16_t j=0; j<6; j++) {
+ // solution violates the consistency_limits, throw it out
+ double abs_diff = std::fabs(ik_seed_state[ur_joint_inds_start_+j] - q_ik_sols[i][j]);
+ if(!consistency_limits.empty() && abs_diff > consistency_limits[ur_joint_inds_start_+j]) {
+ cur_weighted_diff = std::numeric_limits<double>::infinity();
+ break;
+ }
+ cur_weighted_diff += ik_weights_[j] * abs_diff;
+ }
+ weighted_diffs.push_back(idx_double(i, cur_weighted_diff));
+ }
+
+ std::sort(weighted_diffs.begin(), weighted_diffs.end(), comparator);
+
+#if 0
+ printf("start\n");
+ printf(" q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", ik_seed_state[1], ik_seed_state[2], ik_seed_state[3], ik_seed_state[4], ik_seed_state[5], ik_seed_state[6]);
+ for(uint16_t i=0; i<weighted_diffs.size(); i++) {
+ int cur_idx = weighted_diffs[i].first;
+ printf("diff %f, i %d, q %1.2f, %1.2f, %1.2f, %1.2f, %1.2f, %1.2f\n", weighted_diffs[i].second, cur_idx, q_ik_sols[cur_idx][0], q_ik_sols[cur_idx][1], q_ik_sols[cur_idx][2], q_ik_sols[cur_idx][3], q_ik_sols[cur_idx][4], q_ik_sols[cur_idx][5]);
+ }
+ printf("end\n");
+#endif
+
+ for(uint16_t i=0; i<weighted_diffs.size(); i++) {
+ if(weighted_diffs[i].second == std::numeric_limits<double>::infinity()) {
+ // rest are infinity, no more feasible solutions
+ break;
+ }
+
+ // copy the best solution to the output
+ int cur_idx = weighted_diffs[i].first;
+ std::copy(q_ik_sols[cur_idx], q_ik_sols[cur_idx+1], solution.begin() + ur_joint_inds_start_);
+
+ // see if this solution passes the callback function test
+ if(!solution_callback.empty())
+ solution_callback(ik_pose, solution, error_code);
+ else
+ error_code.val = error_code.SUCCESS;
+
+ if(error_code.val == error_code.SUCCESS) {
+#if 0
+ std::vector<std::string> fk_link_names;
+ fk_link_names.push_back(ur_link_names_.back());
+ std::vector<geometry_msgs::Pose> fk_poses;
+ getPositionFK(fk_link_names, solution, fk_poses);
+ KDL::Frame kdl_fk_pose;
+ tf::poseMsgToKDL(fk_poses[0], kdl_fk_pose);
+ printf("FK(solution) - pose \n");
+ for(int i=0; i<4; i++) {
+ for(int j=0; j<4; j++)
+ printf("%1.6f ", kdl_fk_pose(i, j)-kdl_ik_pose(i, j));
+ printf("\n");
+ }
+#endif
+ return true;
+ }
+ }
+ // none of the solutions were both consistent and passed the solution callback
+
+ if(options.lock_redundant_joints) {
+ ROS_DEBUG_NAMED("kdl","Will not pertubate redundant joints to find solution");
+ break;
+ }
+
+ if(dimension_ == 6) {
+ ROS_DEBUG_NAMED("kdl","No other joints to pertubate, cannot find solution");
+ break;
+ }
+
+ // randomly pertubate other joints and try again
+ if(!consistency_limits.empty()) {
+ getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_test, false);
+ } else {
+ getRandomConfiguration(jnt_pos_test, false);
+ }
+ }
+
+ ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
+ error_code.val = error_code.NO_IK_SOLUTION;
+ return false;
+}
+
+bool URKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
+ const std::vector<double> &joint_angles,
+ std::vector<geometry_msgs::Pose> &poses) const
+{
+ ros::WallTime n1 = ros::WallTime::now();
+ if(!active_)
+ {
+ ROS_ERROR_NAMED("kdl","kinematics not active");
+ return false;
+ }
+ poses.resize(link_names.size());
+ if(joint_angles.size() != dimension_)
+ {
+ ROS_ERROR_NAMED("kdl","Joint angles vector must have size: %d",dimension_);
+ return false;
+ }
+
+ KDL::Frame p_out;
+ geometry_msgs::PoseStamped pose;
+ tf::Stamped<tf::Pose> tf_pose;
+
+ KDL::JntArray jnt_pos_in(dimension_);
+ for(unsigned int i=0; i < dimension_; i++)
+ {
+ jnt_pos_in(i) = joint_angles[i];
+ }
+
+ KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
+
+ bool valid = true;
+ for(unsigned int i=0; i < poses.size(); i++)
+ {
+ ROS_DEBUG_NAMED("kdl","End effector index: %d",getKDLSegmentIndex(link_names[i]));
+ if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
+ {
+ tf::poseKDLToMsg(p_out,poses[i]);
+ }
+ else
+ {
+ ROS_ERROR_NAMED("kdl","Could not compute FK for %s",link_names[i].c_str());
+ valid = false;
+ }
+ }
+ return valid;
+}
+
+const std::vector<std::string>& URKinematicsPlugin::getJointNames() const
+{
+ return ik_chain_info_.joint_names;
+}
+
+const std::vector<std::string>& URKinematicsPlugin::getLinkNames() const
+{
+ return ik_chain_info_.link_names;
+}
+
+} // namespace
diff --git a/ur_kinematics/ur_moveit_plugins.xml b/ur_kinematics/ur_moveit_plugins.xml
new file mode 100644
index 0000000000000000000000000000000000000000..abcdd057592d7ab29036e0d036b05f0af85c141d
--- /dev/null
+++ b/ur_kinematics/ur_moveit_plugins.xml
@@ -0,0 +1,19 @@
+<library path="lib/libur10_moveit_plugin">
+ <class name="ur_kinematics/UR10KinematicsPlugin" type="ur_kinematics::URKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+ <description>
+ Analytic kinematics for the Universal Robots UR10.
+ Developed by Kelsey Hawkins from Georgia Tech.
+ See http://hdl.handle.net/1853/50782 for details.
+ </description>
+ </class>
+</library>
+
+<library path="lib/libur5_moveit_plugin">
+ <class name="ur_kinematics/UR5KinematicsPlugin" type="ur_kinematics::URKinematicsPlugin" base_class_type="kinematics::KinematicsBase">
+ <description>
+ Analytic kinematics for the Universal Robots UR5.
+ Developed by Kelsey Hawkins from Georgia Tech.
+ See http://hdl.handle.net/1853/50782 for details.
+ </description>
+ </class>
+</library>