New store saved state (#835)

* Removing static UR paths (#828) * removed static paths to scripts * undid change * improved comments * unified * returning at the correct time (#830) * added Acceleration_Scaling_Factor and Velocity_Scaling_Factor = 0.5 to all .rviz config (#832) * adding new store state for pelicase * adding new stored state called store * adapting the pack/store for new palm flesh * rewriting the date --------- Co-authored-by: Jesús Ferrándiz <[email protected]>
shadow-robot · Jun 27, 2023 · 749210b · 749210b
1 parent 067197e
commit 749210b
Show file tree

Hide file tree

Showing 4 changed files with 118 additions and 7 deletions.
diff --git a/sr_demos/scripts/sr_demos/store.py b/sr_demos/scripts/sr_demos/store.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+
+# Software License Agreement (BSD License)
+# Copyright © 2023 belongs to Shadow Robot Company Ltd.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without modification,
+# are permitted provided that the following conditions are met:
+#   1. Redistributions of source code must retain the above copyright notice,
+#      this list of conditions and the following disclaimer.
+#   2. 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.
+#   3. Neither the name of Shadow Robot Company Ltd 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 Shadow Robot Company Ltd "as is" and any express
+# or implied warranties, including, but not limited to, the implied warranties of
+# merchantability and fitness for a particular purpose are disclaimed. In no event
+# shall the copyright holder 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.
+
+import argparse
+import rospy
+from sr_robot_commander.sr_hand_commander import SrHandCommander
+
+
+if __name__ == "__main__":
+
+    rospy.init_node("store_hand", anonymous=True)
+
+    parser = argparse.ArgumentParser(description="Hand side")
+    parser.add_argument("-s", "--side",
+                        dest="side",
+                        type=str,
+                        required=True,
+                        help="Please select hand side, can be 'right', 'left' or 'both'.",
+                        default=None,
+                        choices=["right", "left", "both"])
+
+    args = parser.parse_args(rospy.myargv()[1:])
+
+    if args.side == 'right':
+        hand_name = 'right_hand'
+    elif args.side == 'left':
+        hand_name = 'left_hand'
+    else:
+        hand_name = 'two_hands'
+
+    trajectory = [
+        {
+            'name': 'open',
+            'interpolate_time': 3.0,
+            'pause_time': 2
+        },
+        {
+            'name': 'store',
+            'interpolate_time': 3.0,
+            'pause_time': 2
+        }
+    ]
+
+    commander_instance = SrHandCommander(hand_name)
+    commander_instance.run_named_trajectory(trajectory)
diff --git a/sr_moveit_hand_config/config/shadowhand.srdf.xacro b/sr_moveit_hand_config/config/shadowhand.srdf.xacro
@@ -129,11 +129,11 @@
     </xacro:unless>
     <xacro:if value="${hand_version == 'E4'}">
       <xacro:if value="${tip_sensors == 'pst'}">
-        <joint name="${prefix}${joint_prefix}J1" value="1.5707" />
+        <joint name="${prefix}${joint_prefix}J1" value="1.4" />
         <joint name="${prefix}${joint_prefix}J3" value="1.309" />
       </xacro:if>
       <xacro:if value="${tip_sensors == 'bt_sp'}">
-        <joint name="${prefix}${joint_prefix}J1" value="1.309" />
+        <joint name="${prefix}${joint_prefix}J1" value="1.000" />
         <joint name="${prefix}${joint_prefix}J3" value="1.309" />
       </xacro:if>
       <joint name="${prefix}${joint_prefix}J2" value="1.5707" />
@@ -224,6 +224,16 @@
     </xacro:if> 
   </xacro:macro>
 
+  <xacro:macro name="thumb_state_store" params="prefix is_lite">
+    <joint name="${prefix}THJ1" value="0.15708" />
+    <joint name="${prefix}THJ2" value="0.541052" />
+    <xacro:unless value="$(arg is_lite)">
+      <joint name="${prefix}THJ3" value="0.122173" />
+    </xacro:unless>
+    <joint name="${prefix}THJ4" value="0.331613" />
+    <joint name="${prefix}THJ5" value="0.331613" />
+  </xacro:macro>
+
   <xacro:macro name="thumb_state_relaxed" params="prefix">
     <joint name="${prefix}THJ1" value="0.0837" />
     <joint name="${prefix}THJ2" value="0.2629" />

diff --git a/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro b/sr_moveit_hand_config/config/shadowhands_prefix.srdf.xacro
@@ -173,7 +173,7 @@
       <joint name="$(arg prefix)WRJ2" value="0" />
     </xacro:unless>
   </group_state>
-  
+
   <group_state name="fingers_pack_thumb_open" group="$(arg hand_name)">
     <xacro:if value="$(arg th)">
         <xacro:thumb_state_open  prefix="$(arg prefix)" />
@@ -369,6 +369,32 @@
       <joint name="$(arg prefix)WRJ2" value="0" />
     </xacro:unless>
   </group_state>
+  <group_state name="store" group="$(arg hand_name)">
+    <xacro:if value="$(arg th)">
+      <xacro:unless value="$(arg is_lite)">
+        <xacro:thumb_state_store  prefix="$(arg prefix)" is_lite="$(arg is_lite)"/>
+      </xacro:unless>
+      <xacro:if value="$(arg is_lite)">
+        <xacro:thumb_state_pack  prefix="$(arg prefix)" tip_sensors="$(arg tip_sensors)" hand_version="$(arg hand_version)"/>
+      </xacro:if>
+    </xacro:if>
+    <xacro:if value="$(arg ff)">
+        <xacro:finger_state_pack  prefix="$(arg prefix)" joint_prefix="FF" lf="0" tip_sensors="$(arg tip_sensors)" hand_version="$(arg hand_version)"/>
+    </xacro:if>
+    <xacro:if value="$(arg mf)">
+        <xacro:finger_state_pack  prefix="$(arg prefix)" joint_prefix="MF" lf="0" tip_sensors="$(arg tip_sensors)" hand_version="$(arg hand_version)"/>
+    </xacro:if>
+    <xacro:if value="$(arg rf)">
+        <xacro:finger_state_pack  prefix="$(arg prefix)" joint_prefix="RF" lf="0" tip_sensors="$(arg tip_sensors)" hand_version="$(arg hand_version)"/>
+    </xacro:if>
+    <xacro:unless value="$(arg is_lite)">
+      <xacro:if value="$(arg lf)">
+          <xacro:finger_state_pack  prefix="$(arg prefix)" joint_prefix="LF" lf="1" tip_sensors="$(arg tip_sensors)" hand_version="$(arg hand_version)"/>
+      </xacro:if>
+      <joint name="$(arg prefix)WRJ1" value="0" />
+      <joint name="$(arg prefix)WRJ2" value="0" />
+    </xacro:unless>
+  </group_state>
 
   <!--END EFFECTOR: Purpose: Represent information about an end effector.-->
 

diff --git a/sr_multi_moveit/sr_multi_moveit_config/config/multi_robot_move_group_states.yaml b/sr_multi_moveit/sr_multi_moveit_config/config/multi_robot_move_group_states.yaml
@@ -85,13 +85,13 @@ left_arm_and_hand:
       - move_group: left_arm_and_wrist
         move_group_state: hand_change
       - move_group: left_hand
-        move_group_state: pack
+        move_group_state: store
   safe_store:
     inherit_from:
       - move_group: left_arm
         move_group_state: safe_store
       - move_group: left_hand
-        move_group_state: open
+        move_group_state: store
 right_arm:
   ra_start:
     joint_angles:
@@ -152,13 +152,13 @@ right_arm_and_hand:
       - move_group: right_arm_and_wrist
         move_group_state: hand_change
       - move_group: right_hand
-        move_group_state: pack
+        move_group_state: store
   safe_store:
     inherit_from:
       - move_group: right_arm
         move_group_state: safe_store
       - move_group: right_hand
-        move_group_state: open
+        move_group_state: store
 two_arms:
   start:
     inherit_from:
@@ -203,6 +203,12 @@ two_hands:
         move_group_state: relaxed
       - move_group: left_hand
         move_group_state: relaxed
+  store:
+    inherit_from:
+      - move_group: right_hand
+        move_group_state: store
+      - move_group: left_hand
+        move_group_state: store
 two_arms_and_wrists:
   start:
     inherit_from: