Inverse Kinematics Sim Service

This update adds the SNS IK solver as the backbone of the IK service
inside the Kinematics class. Updates include:

- Added sns_ik_lib as a dependency
- Acceleration Limits to be added to the parameter server
- Distinguishes between the endpoint tips and gravity compensation tips
  (to properly account for gravity)
- Adds the servicePositionIK ROS Service

sawyer_gazebo/CMakeLists.txt

@@ -15,6 +15,7 @@ find_package(catkin
     std_msgs
     kdl_parser
     tf_conversions
+    sns_ik_lib
 )
 
 # Depend on system install of Gazebo
@@ -31,6 +32,7 @@ catkin_package(
   roscpp
   kdl_parser
   tf_conversions
+  sns_ik_lib
  INCLUDE_DIRS
   include
  LIBRARIES

sawyer_gazebo/config/acceleration_limits.yaml

@@ -0,0 +1,12 @@
+robot_config:
+    joint_config:
+        joint_acceleration_limit:
+            head_pan: 10.0 # rad / sec^2
+            right_j0: 10.0 # rad / sec^2
+            right_j1: 8.0  # rad / sec^2
+            right_j2: 10.0 # rad / sec^2
+            right_j3: 10.0 # rad / sec^2
+            right_j4: 12.0 # rad / sec^2
+            right_j5: 12.0 # rad / sec^2
+            right_j6: 12.0 # rad / sec^2
+

sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h

@@ -31,9 +31,11 @@
 #include <kdl/chainidsolver_recursive_newton_euler.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainfksolvervel_recursive.hpp>
+#include <sns_ik/sns_ik.hpp>
 
 #include <kdl/jntarray.hpp>
 #include <kdl/tree.hpp>
+#include <urdf/model.h>
 
 namespace sawyer_gazebo
 {
@@ -47,19 +49,21 @@ class ArmKinematicsInterface
   */
 bool init(ros::NodeHandle& nh, std::string side);
 
-private:
-std::string side_, root_name_, tip_name_, camera_name_;
-KDL::Tree tree_;
 struct Kinematics
 {
-    KDL::Chain chain;
-    std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver;
-    std::unique_ptr<KDL::ChainFkSolverVel_recursive> fk_vel_solver;
-    std::unique_ptr<KDL::ChainIdSolver_RNE>         gravity_solver;
-    /*std::unique_ptr<KDL::ChainFDSolverTau>           fk_eff_solver; TODO(imcmahon)
-    std::unique_ptr<KDL::ChainIkSolverVel_pinv> ik_solver_vel;
-    std::unique_ptr<KDL::ChainIkSolverPos_NR_JL> ik_solver_pos;*/
+  KDL::Chain chain;
+  std::vector<std::string> joint_names;
+  std::unique_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver;
+  std::unique_ptr<KDL::ChainFkSolverVel_recursive> fk_vel_solver;
+  std::unique_ptr<KDL::ChainIdSolver_RNE>         gravity_solver;
+  std::unique_ptr<sns_ik::SNS_IK>                     ik_solver;
+  /*std::unique_ptr<KDL::ChainFDSolverTau>           fk_eff_solver; TODO(imcmahon)*/
 };
+private:
+std::string side_, root_name_, tip_name_, camera_name_, gravity_tip_name_;
+urdf::Model robot_model_;
+KDL::Tree tree_;
+std::map<std::string, double> acceleration_map_;
 std::map<std::string, Kinematics> kinematic_chain_map_;
 
 realtime_tools::RealtimeBox< std::shared_ptr<const intera_core_msgs::JointCommand> > joint_command_buffer_;
@@ -94,6 +98,8 @@ void jointCommandCallback(const intera_core_msgs::JointCommandConstPtr& msg);
  */
 void jointStateCallback(const sensor_msgs::JointStateConstPtr& msg);
 
+/* Method to publish the Gravity Compensation Torques
+ */
 void publishGravityTorques(); // TODO(imcmahon): publish gravity
 
 /* Method to publish the endpoint state message
@@ -112,7 +118,10 @@ bool parseParams(const ros::NodeHandle& nh);
 bool servicePositionFK(intera_core_msgs::SolvePositionFK::Request& req,
                        intera_core_msgs::SolvePositionFK::Response& res);
 
-/* TODO(imcmahon): IK Service */
+/* Method to service the Inverse Kinematics request of any
+ * kinematic chain starting at the "base"
+ * @returns true to conform to ROS Service signature
+ */
 bool servicePositionIK(intera_core_msgs::SolvePositionIK::Request& req,
                        intera_core_msgs::SolvePositionIK::Response& res);
 
@@ -122,6 +131,15 @@ bool servicePositionIK(intera_core_msgs::SolvePositionIK::Request& req,
  */
 bool computePositionFK(const Kinematics& kin, const KDL::JntArray& jnt_pos, geometry_msgs::Pose& result);
 
+/* Method to calculate the position IK for the required geometry_msg::Pose
+ * with the result stored in KDL::JntArray
+ *  @returns true if successful
+ */
+bool computePositionIK(const Kinematics& kin, const geometry_msgs::Pose& cart_pose,
+                       const KDL::JntArray& jnt_nullspace_bias,
+                       const KDL::JntArray& jnt_seed, KDL::JntArray& result);
+
+
 /* Method to calculate the velocity FK for the required joint velocities in rad/sec
  * with the result stored in geometry_msgs::Twist
  * @returns true if successful
@@ -138,9 +156,19 @@ bool computeGravityFK(const Kinematics& kin, const KDL::JntArray& jnt_pos,
 /* Method to break down a JointState message object into the corresponding
  * KDL position, velocity, and effort Joint Arrays
  */
-void jointStateToKDL(const sensor_msgs::JointState& joint_configuration, const KDL::Chain& chain,
-                     KDL::JntArray& jnt_pos, KDL::JntArray& jnt_vel, KDL::JntArray& jnt_eff,
-                     std::vector<std::string>& jnt_names);
+void jointStateToKDL(const sensor_msgs::JointState& joint_configuration, const Kinematics& kin,
+                     KDL::JntArray& jnt_pos, KDL::JntArray& jnt_vel, KDL::JntArray& jnt_eff);
+
+/* Method to break down a JointState message object into the corresponding
+ * the KDL position Joint Array
+ */
+void jointStatePositionToKDL(const sensor_msgs::JointState& joint_configuration,
+                             const Kinematics& kin, KDL::JntArray& jnt_pos)
+{
+  KDL::JntArray jnt_vel, jnt_eff;
+  jointStateToKDL(joint_configuration, kin, jnt_pos, jnt_vel, jnt_eff);
+};
+
 };
 }  // namespace sawyer_gazebo
 #endif  // SAWYER_GAZEBO_ARM_KINEMATICS_INTERFACE_H

sawyer_gazebo/launch/sawyer_world.launch

@@ -9,8 +9,8 @@
   <arg name="debug" default="false"/>
   <arg name="head_display_img" default="$(find sawyer_gazebo)/share/images/sawyer_sdk_research.png"/>
 
-  <!-- These arguments load the electric grippers, for example right_electric_gripper:=true -->
-  <arg name="right_electric_gripper" default="false"/>
+  <!-- These arguments load the electric grippers, for example electric_gripper:=true -->
+  <arg name="electric_gripper" default="false"/>
 
   <param name="img_path_head_display" value="$(arg head_display_img)"/>
 
@@ -20,13 +20,19 @@
              to launching sawyer_world -->
   <arg name="load_robot_description" default="true"/>
   <param if="$(arg load_robot_description)" name="robot_description"
-      command="$(find xacro)/xacro --inorder $(find sawyer_description)/urdf/sawyer.urdf.xacro gazebo:=true"/>
+      command="$(find xacro)/xacro --inorder $(find sawyer_description)/urdf/sawyer.urdf.xacro
+      gazebo:=true electric_gripper:=$(arg electric_gripper)"/>
   <!-- Load Parameters to the ROS Parameter Server -->
+  <rosparam command="load" file="$(find sawyer_gazebo)/config/config.yaml" />
   <rosparam command="load" file="$(find sawyer_description)/params/named_poses.yaml" />
-  <rosparam command="load" file="$(find sawyer_gazebo)/config/params.yaml" />
+  <rosparam command="load" file="$(find sawyer_gazebo)/config/acceleration_limits.yaml" />
   <param name="robot/limb/right/root_name" value="base" />
   <param name="robot/limb/right/tip_name" value="right_hand" />
   <param name="robot/limb/right/camera_name" value="right_hand_camera" />
+  <param if="$(arg electric_gripper)"     name="robot/limb/right/gravity_tip_name"
+         value="right_gripper_tip" />
+  <param unless="$(arg electric_gripper)" name="robot/limb/right/gravity_tip_name"
+         value="right_hand" />
 
 
   <!-- We resume the logic in empty_world.launch, changing the name of the world to be launched -->
@@ -55,7 +61,7 @@
 
   <!-- ros_control sawyer launch file -->
   <include file="$(find sawyer_sim_controllers)/launch/sawyer_sim_controllers.launch">
-      <arg name="right_electric_gripper" value="$(arg right_electric_gripper)"/>
+      <arg name="electric_gripper" value="$(arg electric_gripper)"/>
       <arg name="gui" value="$(arg gui)" />
   </include>
 

sawyer_gazebo/package.xml

@@ -52,6 +52,7 @@
   <build_depend>kdl_parser</build_depend>
   <build_depend>tf_conversions</build_depend>
   <build_depend>roslint</build_depend>
+  <build_depend>sns_ik_lib</build_depend>
   <build_depend>sawyer_hardware_interface</build_depend>
 
   <run_depend>cv_bridge</run_depend>
@@ -68,6 +69,7 @@
   <run_depend>tf_conversions</run_depend>
   <run_depend>sawyer_hardware_interface</run_depend>
   <run_depend>image_proc</run_depend>
+  <run_depend>sns_ik_lib</run_depend>
   <run_depend>nodelet</run_depend>