Inverse Kinematics Service update #18

IanTheEngineer · 2017-10-26T18:58:06Z

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

rethink-forrest

It looks mostly there.

rethink-forrest · 2017-10-26T20:27:04Z

sawyer_gazebo/config/acceleration_limits.yaml

@@ -0,0 +1,12 @@
+robot_config:
+    joint_config:
+        joint_acceleration_limit:


Is there anyway of specifying units here?

Yep! Comments are supported. Will do

rethink-forrest · 2017-10-26T20:29:07Z

sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h

+  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)
+  std::unique_ptr<KDL::ChainIkSolverVel_pinv> ik_solver_vel;


A velocity IK solver should be simple to add via SNS, or did you want to save this for future work.

rethink-forrest · 2017-10-26T20:29:33Z

sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h

+  std::unique_ptr<sns_ik::SNS_IK>                     ik_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;*/


The SHS IK solver replaces this.

rethink-forrest · 2017-10-26T20:31:19Z

sawyer_gazebo/include/sawyer_gazebo/arm_kinematics_interface.h

@@ -122,6 +128,11 @@ bool servicePositionIK(intera_core_msgs::SolvePositionIK::Request& req,
 */
 bool computePositionFK(const Kinematics& kin, const KDL::JntArray& jnt_pos, geometry_msgs::Pose& result);

+bool computePositionIK(const Kinematics& kin, const geometry_msgs::Pose& cart_pose,


There function parameters need to be documented.

rethink-forrest · 2017-10-26T20:32:40Z

sawyer_gazebo/launch/sawyer_world.launch

@@ -20,13 +20,16 @@
             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)"/>


Check line length.

rethink-forrest · 2017-10-26T21:00:18Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+    {
+      seed_modes.push_back(req.seed_mode);
+    }
+    for(size_t j=0; j<seed_modes.size(); j++)


Creating a vector for seed_modes is really confusing. There are only two options here, with SEED_AUTO trying both. I feel like there is a better way of structuring this.

Agreed. Restructured the flow on this to be more intuitive, though it will function the same.

rethink-forrest · 2017-10-26T21:01:53Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+      else // (req.seed_mode == req.SEED_CURRENT)
+      {
+        std::shared_ptr<const sensor_msgs::JointState> joint_state;
+        joint_state_buffer_.get(joint_state);


Is this measured or reference joint states?

There are only measured joint states in sim. Though I could consider adding reference.

rethink-forrest · 2017-10-26T21:02:41Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+        }
+      }
+
+      if (!computePositionIK(kinematic_chain_map_[req.tip_names[i]], req.pose_stamp[i].pose, jnt_nullspace_bias, jnt_seed, jnt_result))


check line length

rethink-forrest · 2017-10-26T21:04:31Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+        /* if (jointsInCollision(kinematic_chain_map_[req.tip_names[i]], jnt_result))
+        {
+          res.result_type[i] = res.IK_IN_COLLISION;
+        }  TODO(imcmahon) Utilize FCL for collision checking


Is there a issue filed for this?

rethink-forrest · 2017-10-26T21:07:23Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+          req.seed_angles[i].name.size() == req.seed_angles[i].position.size())
+      {
+
+        jointStatePositionToKDL(req.seed_angles[i], kinematic_chain_map_[req.tip_names[i]], jnt_seed);


Only search kinematic_chain_map_ once for req.tip_names[i].

So, this isn't actually a search, but directly accessing the hash of a mapped variable. I can't easily copy this to a variable as most of the contents are stored in a unique_ptr variable, which cannot be copied without transferring ownership.

Under the hood, getting any element from a map requires a search based on the hash. You might also want to be caution that the [] will create a new element if one doesn't already exist for the desired key.
http://www.cplusplus.com/reference/map/map/operator[]/

You could also store the map iterator if your worried about copying objects.

The way the code is currently structured, no element is accessed without already existing. It is possible that the code could be misused in the future though. The unique_ptrs cannot be copied. I'm not particularly inclined to optimize around the hash lookup costs, but I think your iterator idea is an excellent one. I'll explore that thought.

rethink-forrest · 2017-10-27T20:01:07Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

+           (res.result_type[i] == res.IK_FAILED || res.result_type[i] == res.IK_IN_COLLISION)
+         )
+       )
+    {
        std::shared_ptr<const sensor_msgs::JointState> joint_state;
        joint_state_buffer_.get(joint_state);
        if (joint_state.get())


Print warning if joint_state doesn't exist.

rethink-forrest

Looks good.

rethink-forrest · 2017-10-27T20:01:52Z

sawyer_gazebo/src/arm_kinematics_interface.cpp

-      seed_modes.push_back(req.SEED_CURRENT);
-    }
-    else
+    if (req.seed_mode == req.SEED_USER || req.seed_mode == req.SEED_AUTO)


This looks much cleaner.

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

IanTheEngineer requested a review from rethink-rlinsalata October 26, 2017 18:58

IanTheEngineer added the enhancement label Oct 26, 2017

IanTheEngineer added this to the SDK 5.2 milestone Oct 26, 2017

IanTheEngineer force-pushed the kinematics_update branch from 656aea7 to 00e835c Compare October 26, 2017 20:38

rethink-forrest suggested changes Oct 26, 2017

View reviewed changes

rethink-forrest reviewed Oct 27, 2017

View reviewed changes

rethink-forrest approved these changes Oct 27, 2017

View reviewed changes

IanTheEngineer force-pushed the kinematics_update branch from c7bb488 to 26b4fa3 Compare October 27, 2017 21:12

IanTheEngineer merged commit a084be6 into development Oct 27, 2017

IanTheEngineer deleted the kinematics_update branch October 30, 2017 15:57

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Inverse Kinematics Service update #18

Inverse Kinematics Service update #18

IanTheEngineer commented Oct 26, 2017

rethink-forrest left a comment

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

rethink-forrest Oct 26, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 26, 2017

IanTheEngineer Oct 27, 2017

rethink-forrest Oct 27, 2017

IanTheEngineer Oct 27, 2017 •

edited

Loading

rethink-forrest Oct 27, 2017

rethink-forrest left a comment

rethink-forrest Oct 27, 2017

Inverse Kinematics Service update #18

Inverse Kinematics Service update #18

Conversation

IanTheEngineer commented Oct 26, 2017

rethink-forrest left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer Oct 27, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

rethink-forrest left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

IanTheEngineer Oct 27, 2017 •

edited

Loading