From fa7d34dad421cb20fd66eaa64522918696b9aa04 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 11 Oct 2016 15:16:58 +0900
Subject: [PATCH 1/3] add arg grav_[right|left]_name and [r|l]_tip_name

---
 baxter_sim_hardware/launch/baxter_sdk_control.launch | 11 ++++++++++-
 .../launch/baxter_sim_kinematics.launch              | 12 ++++++++----
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/baxter_sim_hardware/launch/baxter_sdk_control.launch b/baxter_sim_hardware/launch/baxter_sdk_control.launch
index 951a906..52ca96c 100644
--- a/baxter_sim_hardware/launch/baxter_sdk_control.launch
+++ b/baxter_sim_hardware/launch/baxter_sdk_control.launch
@@ -3,9 +3,18 @@
 
   <arg name="left_electric_gripper" default="true"/>
   <arg name="right_electric_gripper" default="true"/>
+  <arg name="grav_right_name" default="right_gripper" />
+  <arg name="grav_left_name" default="left_gripper" />
+  <arg name="right_tip_name" default="right_gripper" />
+  <arg name="left_tip_name" default="left_gripper" />
 
   <!-- baxter_sim_kinematics launch file to do the Forward/Inverse Kinematics -->
-  <include file="$(find baxter_sim_kinematics)/launch/baxter_sim_kinematics.launch" />
+  <include file="$(find baxter_sim_kinematics)/launch/baxter_sim_kinematics.launch">
+    <arg name="grav_right_name" value="$(arg grav_right_name)" />
+    <arg name="grav_left_name" value="$(arg grav_left_name)" />
+    <arg name="right_tip_name" value="$(arg right_tip_name)" />
+    <arg name="left_tip_name" value="$(arg left_tip_name)" />
+  </include>
 
   <!-- Load joint controller configurations from YAML file to parameter server -->
   <rosparam file="$(find baxter_sim_hardware)/config/baxter_sim_controllers.yaml" command="load"/>
diff --git a/baxter_sim_kinematics/launch/baxter_sim_kinematics.launch b/baxter_sim_kinematics/launch/baxter_sim_kinematics.launch
index dd67e40..40e2966 100644
--- a/baxter_sim_kinematics/launch/baxter_sim_kinematics.launch
+++ b/baxter_sim_kinematics/launch/baxter_sim_kinematics.launch
@@ -5,6 +5,10 @@
   <arg name="load_robot_description" default="false"/>
   <arg name="right_electric_gripper" default="false"/>
   <arg name="left_electric_gripper" default="false"/>
+  <arg name="grav_right_name" default="right_gripper" />
+  <arg name="grav_left_name" default="left_gripper" />
+  <arg name="right_tip_name" default="right_gripper" />
+  <arg name="left_tip_name" default="left_gripper" />
 
   <!-- Load universal robotic description format (URDF) -->
   <param if="$(arg load_robot_description)" name="robot_description"
@@ -13,11 +17,11 @@
           left_electric_gripper:=$(arg left_electric_gripper)"/>
 
   <param name="root_name" value="base" />
-  <param name="grav_right_name" value="right_gripper" />
-  <param name="grav_left_name" value="left_gripper" />
+  <param name="grav_right_name" value="$(arg grav_right_name)" />
+  <param name="grav_left_name" value="$(arg grav_left_name)" />
   <!-- load left and right tip name -->
-  <param name="right_tip_name" value="right_gripper" />
-  <param name="left_tip_name" value="left_gripper" />
+  <param name="right_tip_name" value="$(arg right_tip_name)" />
+  <param name="left_tip_name" value="$(arg left_tip_name)" />
   <!-- load left and right joint names -->
   <rosparam param="robot_config/right_config/joint_names">
          [right_s0, right_s1, right_e0, right_e1, right_w0, right_w1, right_w2]

From 80d47cc244004fb69ff4af54ac03d6b049876be4 Mon Sep 17 00:00:00 2001
From: Shingo Kitagawa <shingogo@hotmail.co.jp>
Date: Tue, 11 Oct 2016 20:08:13 +0900
Subject: [PATCH 2/3] do not load gripper description when arg is false

---
 baxter_gazebo/launch/baxter_world.launch            |  2 +-
 .../launch/baxter_sdk_control.launch                | 13 +++++++++----
 2 files changed, 10 insertions(+), 5 deletions(-)

diff --git a/baxter_gazebo/launch/baxter_world.launch b/baxter_gazebo/launch/baxter_world.launch
index ac43ae6..4bbc76f 100644
--- a/baxter_gazebo/launch/baxter_world.launch
+++ b/baxter_gazebo/launch/baxter_world.launch
@@ -19,7 +19,7 @@
              to launching baxter_world -->
   <arg name="load_robot_description" default="true"/>
   <param if="$(arg load_robot_description)" name="robot_description"
-      command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=true"/>
+      command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=true right_electric_gripper:=$(arg right_electric_gripper) left_electric_gripper:=$(arg left_electric_gripper)"/>
 
   <!-- We resume the logic in empty_world.launch, changing the name of the world to be launched -->
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
diff --git a/baxter_sim_hardware/launch/baxter_sdk_control.launch b/baxter_sim_hardware/launch/baxter_sdk_control.launch
index 52ca96c..6777b1c 100644
--- a/baxter_sim_hardware/launch/baxter_sdk_control.launch
+++ b/baxter_sim_hardware/launch/baxter_sdk_control.launch
@@ -3,10 +3,15 @@
 
   <arg name="left_electric_gripper" default="true"/>
   <arg name="right_electric_gripper" default="true"/>
-  <arg name="grav_right_name" default="right_gripper" />
-  <arg name="grav_left_name" default="left_gripper" />
-  <arg name="right_tip_name" default="right_gripper" />
-  <arg name="left_tip_name" default="left_gripper" />
+  <arg name="grav_right_name" default="right_gripper" if="$(arg right_electric_gripper)" />
+  <arg name="grav_left_name" default="left_gripper" if="$(arg left_electric_gripper)" />
+  <arg name="right_tip_name" default="right_gripper" if="$(arg right_electric_gripper)" />
+  <arg name="left_tip_name" default="left_gripper" if="$(arg left_electric_gripper)" />
+  <arg name="grav_right_name" default="right_wrist" unless="$(arg right_electric_gripper)" />
+  <arg name="grav_left_name" default="left_wrist" unless="$(arg left_electric_gripper)" />
+  <arg name="right_tip_name" default="right_wrist" unless="$(arg right_electric_gripper)" />
+  <arg name="left_tip_name" default="left_wrist" unless="$(arg left_electric_gripper)" />
+
 
   <!-- baxter_sim_kinematics launch file to do the Forward/Inverse Kinematics -->
   <include file="$(find baxter_sim_kinematics)/launch/baxter_sim_kinematics.launch">

From 71dd6dfbfcc9a3ceb49f708c96579a1d39d1909f Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Mon, 31 Jul 2017 18:28:18 +0900
Subject: [PATCH 3/3] set commanded_position from JointState msg

/robot/limb/left/joint_command : thats is not intepolated joint positions, controller in gazebo do that
---
 baxter_sim_hardware/src/baxter_emulator.cpp | 12 ++++++++++++
 1 file changed, 12 insertions(+)

diff --git a/baxter_sim_hardware/src/baxter_emulator.cpp b/baxter_sim_hardware/src/baxter_emulator.cpp
index 4b51f47..c30f48c 100644
--- a/baxter_sim_hardware/src/baxter_emulator.cpp
+++ b/baxter_sim_hardware/src/baxter_emulator.cpp
@@ -502,6 +502,12 @@ void baxter_emulator::reset_head_nod(const ros::TimerEvent &t) {
 void baxter_emulator::update_jnt_st(const sensor_msgs::JointState &msg) {
   jstate_msg = msg;
   float threshold = 0.0009;
+left_gravity.commanded_position.resize(left_gravity.name.size());
+left_gravity.commanded_velocity.resize(left_gravity.name.size());
+left_gravity.commanded_effort.resize(left_gravity.name.size());
+right_gravity.commanded_position.resize(left_gravity.name.size());
+right_gravity.commanded_velocity.resize(left_gravity.name.size());
+right_gravity.commanded_effort.resize(left_gravity.name.size());
 left_gravity.actual_position.resize(left_gravity.name.size());
 left_gravity.actual_velocity.resize(left_gravity.name.size());
 left_gravity.actual_effort.resize(left_gravity.name.size());
@@ -525,12 +531,18 @@ right_gravity.actual_effort.resize(left_gravity.name.size());
 	else {
 	   for (int j=0;j<left_gravity.name.size();j++) {
 	     if (msg.name[i] == left_gravity.name[j]) {
+	       left_gravity.commanded_position[j] = msg.position[i];
+	       left_gravity.commanded_velocity[j] = msg.velocity[i];
+	       left_gravity.commanded_effort[j] = msg.effort[i];
 	       left_gravity.actual_position[j] = msg.position[i];
 	       left_gravity.actual_velocity[j] = msg.velocity[i];
 	       left_gravity.actual_effort[j] = msg.effort[i];
 	       break;
 	     }
 	     else if(msg.name[i] == right_gravity.name[j]) {
+	       right_gravity.commanded_position[j] = msg.position[i];
+	       right_gravity.commanded_velocity[j] = msg.velocity[i];
+	       right_gravity.commanded_effort[j] = msg.effort[i];
 	       right_gravity.actual_position[j] = msg.position[i];
 	       right_gravity.actual_velocity[j] = msg.velocity[i];
 	       right_gravity.actual_effort[j] = msg.effort[i];