From 495456daa4588a59f2499701d7e4c595b93e9ce4 Mon Sep 17 00:00:00 2001
From: JafarAbdi <cafer.abdi@gmail.com>
Date: Mon, 22 Feb 2021 17:11:29 +0000
Subject: [PATCH] Add IK solver for mobile_base + arm

---
 stretch_moveit_config/config/kinematics.yaml          | 6 +++++-
 stretch_moveit_config/config/stretch_description.srdf | 9 ++++++++-
 2 files changed, 13 insertions(+), 2 deletions(-)

diff --git a/stretch_moveit_config/config/kinematics.yaml b/stretch_moveit_config/config/kinematics.yaml
index 131b7a4..7e4531c 100644
--- a/stretch_moveit_config/config/kinematics.yaml
+++ b/stretch_moveit_config/config/kinematics.yaml
@@ -1,4 +1,8 @@
 stretch_arm:
   kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
   kinematics_solver_search_resolution: 0.005
-  kinematics_solver_timeout: 0.005
\ No newline at end of file
+  kinematics_solver_timeout: 0.005
+mobile_base_arm:
+  kinematics_solver: stretch_kinematics_plugin/StretchKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.1
\ No newline at end of file
diff --git a/stretch_moveit_config/config/stretch_description.srdf b/stretch_moveit_config/config/stretch_description.srdf
index efba6cb..42de532 100644
--- a/stretch_moveit_config/config/stretch_description.srdf
+++ b/stretch_moveit_config/config/stretch_description.srdf
@@ -43,10 +43,17 @@
         <joint name="joint_lift" value="1.1"/>
         <joint name="joint_wrist_yaw" value="4"/>
     </group_state>
+    <group name="position">
+        <joint name="position" />
+    </group>
+    <group name="mobile_base_arm">
+        <group name="stretch_arm" />
+        <group name="position" />
+    </group>
     <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
     <end_effector name="gripper" parent_link="link_grasp_center" group="stretch_arm"/>
     <!--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="position" type="floating" parent_frame="map" child_link="base_link"/>
+    <virtual_joint name="position" type="planar" parent_frame="map" 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="camera_link" reason="Never"/>
     <disable_collisions link1="base_link" link2="laser" reason="Adjacent"/>