From 94070a77159244437b2789b0b8dfa12a04a3fabf Mon Sep 17 00:00:00 2001
From: Gregory Izatt <gizatt@mit.edu>
Date: Mon, 26 Jan 2015 23:03:06 -0500
Subject: [PATCH] Minor mods to Atlas frame names, fleshing out IRB140

---
 examples/Atlas/Atlas.m              |  4 ++--
 examples/Atlas/urdf/block_hand.urdf | 29 +++++++++++++++++++++++++++++
 examples/IRB140/IRB140.m            | 17 +++++++++++++++--
 examples/IRB140/IRB140Input.m       |  2 +-
 4 files changed, 47 insertions(+), 5 deletions(-)
 create mode 100644 examples/Atlas/urdf/block_hand.urdf

diff --git a/examples/Atlas/Atlas.m b/examples/Atlas/Atlas.m
index 3e23f6c2fa1f..1ba93d96ca21 100644
--- a/examples/Atlas/Atlas.m
+++ b/examples/Atlas/Atlas.m
@@ -77,7 +77,7 @@
         % Sub in handstates for each hand
         % TODO: by name?
         for i=2:obj.getStateFrame().getNumFrames
-          atlas_state_frame = replaceFrameNum(atlas_state_frame,i,atlasFrames.HandState(obj,i,'atlasFrames.HandState'));
+          atlas_state_frame = replaceFrameNum(atlas_state_frame,i,atlasFrames.HandState(obj,i,'HandState'));
         end
       else
         atlas_state_frame = atlasFrames.AtlasState(obj);
@@ -101,7 +101,7 @@
         % Sub in handstates for each hand
         % TODO: by name?
         for i=2:obj.getInputFrame().getNumFrames
-          input_frame = replaceFrameNum(input_frame,i,atlasFrames.HandInput(obj,i,'atlasFrames.HandInput'));
+          input_frame = replaceFrameNum(input_frame,i,atlasFrames.HandInput(obj,i,'HandInput'));
         end
       else
         input_frame = atlasFrames.AtlasInput(obj);
diff --git a/examples/Atlas/urdf/block_hand.urdf b/examples/Atlas/urdf/block_hand.urdf
new file mode 100644
index 000000000000..dd4dad8c22e6
--- /dev/null
+++ b/examples/Atlas/urdf/block_hand.urdf
@@ -0,0 +1,29 @@
+<?xml version="1.0" ?>
+
+<robot name="block_hand" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="block_hand_box">
+    <inertial>
+      <mass value="1.0"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 .5 1"/>
+      </material>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </visual>
+    <collision group="block_hand">
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+      <material name="red">
+        <color rgba="1.0 0.0 .0 1"/>
+      </material>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </collision>
+  </link>
+</robot>
diff --git a/examples/IRB140/IRB140.m b/examples/IRB140/IRB140.m
index d66dd59b9fc6..50a45b9005df 100644
--- a/examples/IRB140/IRB140.m
+++ b/examples/IRB140/IRB140.m
@@ -26,6 +26,8 @@
       obj = obj@TimeSteppingRigidBodyManipulator('',options.dt,options);
       obj.initialized = 1;
       obj = obj.addRobotFromURDF(urdf,options.base_offset,options.base_rpy,options);
+      obj.base_rpy = options.base_rpy;
+      obj.base_offset = options.base_offset;
       
       if (~strcmp(options.hands, 'none'))
         if (strcmp(options.hands, 'robotiq'))
@@ -37,6 +39,10 @@
           % the controllers know what's up
           options_hand.weld_to_link = 7;
           obj = obj.addRobotFromURDF(getFullPathFromRelativePath('../Atlas/urdf/robotiq_box.urdf'), [0.1; 0.0; 0.0], [pi; 0; pi/2], options_hand);
+        elseif (strcmp(options.hands, 'block_hand'))
+          % Box with collision. Good for hitting things with...
+          options_hand.weld_to_link = 7;
+          obj = obj.addRobotFromURDF(getFullPathFromRelativePath('../Atlas/urdf/block_hand.urdf'), [0.1; 0.0; 0.0], [pi; 0; pi/2], options_hand);
         else
           error('unsupported hand type');
         end
@@ -56,11 +62,16 @@
           arm_state_frame = replaceFrameNum(arm_state_frame,1,IRB140State(obj));
           % Sub in handstates for each hand
           % TODO: by name?
-          for i=2:obj.getStateFrame().getNumFrames
+          for i=2:2
             arm_state_frame = replaceFrameNum(arm_state_frame,i,HandState(obj,i,'HandState'));
           end
         else
-          arm_state_frame = IRB140State(obj);
+          arm_state_frame = getStateFrame(obj);
+          if (length(arm_state_frame.coordinates) > length(IRB140State(obj).coordinates))
+            arm_state_frame = replaceFrameNum(arm_state_frame, 1, IRB140State(obj));
+          else
+            arm_state_frame = IRB140State(obj);
+          end
         end
         tsmanip_state_frame = obj.getStateFrame();
         if tsmanip_state_frame.dim>arm_state_frame.dim
@@ -94,5 +105,7 @@
   properties (SetAccess = protected, GetAccess = public)
     hands = 0; % 0, none; 1, Robotiq
     initialized = 0;
+    base_rpy;
+    base_offset;
   end
 end
diff --git a/examples/IRB140/IRB140Input.m b/examples/IRB140/IRB140Input.m
index 23647fb3ef31..b94f076e399a 100644
--- a/examples/IRB140/IRB140Input.m
+++ b/examples/IRB140/IRB140Input.m
@@ -11,7 +11,7 @@
       input_names = manipInputFrame.coordinates;
       input_names = regexprep(input_names,'_motor',''); % remove motor suffix     
       
-      obj = obj@SingletonCoordinateFrame('IRB140',length(input_names),'x',input_names);
+      obj = obj@SingletonCoordinateFrame('IRB140Input',length(input_names),'x',input_names);
     end
   end
 end