JogArm C++ API fixes (moveit#1911)

* Rename JogCppApi->JogCppInterface to match filename * Use public inheritance of JogInterfaceBase * Fix crash due to different Jacobian dimensions * Clang format, update unit test * Fix dim mismatch in decelerateForSingularity * Revert 'static' for several variables
progtologist · Feb 28, 2020 · 6eaa75d · 6eaa75d
1 parent 0abd095
commit 6eaa75d
Show file tree

Hide file tree

Showing 7 changed files with 53 additions and 47 deletions.
diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h
@@ -101,8 +101,9 @@ class JogCalcs
 
   // Possibly calculate a velocity scaling factor, due to proximity of
   // singularity and direction of motion
-  double decelerateForSingularity(const Eigen::VectorXd& commanded_velocity,
-                                  const Eigen::JacobiSVD<Eigen::MatrixXd>& svd);
+  double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity,
+                                             const Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
+                                             const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse);
 
   /**
    * Slow motion down if close to singularity or collision.
@@ -154,9 +155,6 @@ class JogCalcs
 
   JogArmParameters parameters_;
 
-  // For jacobian calculations
-  Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_;
-  Eigen::JacobiSVD<Eigen::MatrixXd> svd_;
   // Use ArrayXd type to enable more coefficient-wise operations
   Eigen::ArrayXd delta_theta_;
 

diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h
@@ -44,15 +44,15 @@
 namespace moveit_jog_arm
 {
 /**
-* Class JogCppApi - This class should be instantiated in a new thread
+* Class JogCppInterface - This class should be instantiated in a new thread
 * See cpp_interface_example.cpp
 */
-class JogCppApi : protected JogInterfaceBase
+class JogCppInterface : public JogInterfaceBase
 {
 public:
-  JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
+  JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor);
 
-  ~JogCppApi();
+  ~JogCppInterface();
 
   void startMainLoop();
 

diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h
@@ -58,19 +58,30 @@ class JogInterfaceBase
 public:
   JogInterfaceBase();
 
+  /** \brief Update the joints of the robot */
   void jointsCB(const sensor_msgs::JointStateConstPtr& msg);
 
-  // Service callback for changing drift dimensions,
-  // e.g. to allow the wrist joint to rotate
+  /**
+   * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely.
+   * This can help avoid singularities.
+   *
+   * @param request the service request
+   * @param response the service response
+   * @return true if the adjustment was made
+   */
   bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req,
                              moveit_msgs::ChangeDriftDimensions::Response& res);
 
-  // Jogging calculation thread
+  /** \brief Start the main calculation thread */
   bool startJogCalcThread();
+
+  /** \brief Stop the main calculation thread */
   bool stopJogCalcThread();
 
-  // Collision checking thread
+  /** \brief Start collision checking */
   bool startCollisionCheckThread();
+
+  /** \brief Stop collision checking */
   bool stopCollisionCheckThread();
 
 protected:

diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp
@@ -66,7 +66,7 @@ int main(int argc, char** argv)
   planning_scene_monitor->startStateMonitor();
 
   // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate.
-  moveit_jog_arm::JogCppApi jog_interface(planning_scene_monitor);
+  moveit_jog_arm::JogCppInterface jog_interface(planning_scene_monitor);
   std::thread jogging_thread([&]() { jog_interface.startMainLoop(); });
 
   // Make a Cartesian velocity message

diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp
@@ -321,28 +321,30 @@ bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared&
   Eigen::VectorXd delta_x = scaleCartesianCommand(cmd);
 
   // Convert from cartesian commands to joint commands
-  jacobian_ = kinematic_state_->getJacobian(joint_model_group_);
+  Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_);
 
   // May allow some dimensions to drift, based on shared_variables.drift_dimensions
   // i.e. take advantage of task redundancy.
   // Remove the Jacobian rows corresponding to True in the vector shared_variables.drift_dimensions
   // Work backwards through the 6-vector so indices don't get out of order
-  for (auto dimension = jacobian_.rows(); dimension >= 0; --dimension)
+  for (auto dimension = jacobian.rows(); dimension >= 0; --dimension)
   {
-    if (shared_variables.drift_dimensions[dimension] && jacobian_.rows() > 1)
+    if (shared_variables.drift_dimensions[dimension] && jacobian.rows() > 1)
     {
-      removeDimension(jacobian_, delta_x, dimension);
+      removeDimension(jacobian, delta_x, dimension);
     }
   }
 
-  svd_ = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian_, Eigen::ComputeThinU | Eigen::ComputeThinV);
-  matrix_s_ = svd_.singularValues().asDiagonal();
-  pseudo_inverse_ = svd_.matrixV() * matrix_s_.inverse() * svd_.matrixU().transpose();
+  Eigen::JacobiSVD<Eigen::MatrixXd> svd =
+      Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV);
+  Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal();
+  Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose();
 
-  delta_theta_ = pseudo_inverse_ * delta_x;
+  delta_theta_ = pseudo_inverse * delta_x;
 
   // If close to a collision or a singularity, decelerate
-  if (!applyVelocityScaling(shared_variables, mutex, delta_theta_, decelerateForSingularity(delta_x, svd_)))
+  if (!applyVelocityScaling(shared_variables, mutex, delta_theta_,
+                            velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)))
   {
     has_warning_ = true;
     suddenHalt(outgoing_command_);
@@ -473,46 +475,43 @@ bool JogCalcs::applyVelocityScaling(const JogArmShared& shared_variables, std::m
 }
 
 // Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion
-double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_velocity,
-                                          const Eigen::JacobiSVD<Eigen::MatrixXd>& svd)
+double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity,
+                                                     const Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
+                                                     const Eigen::MatrixXd& jacobian,
+                                                     const Eigen::MatrixXd& pseudo_inverse)
 {
   double velocity_scale = 1;
+  std::size_t num_dimensions = jacobian.rows();
 
   // Find the direction away from nearest singularity.
   // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity.
   // The sign can flip at any time, so we have to do some extra checking.
   // Look ahead to see if the Jacobian's condition will decrease.
-  Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(5);
+  Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1);
 
   double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1);
 
   // This singular vector tends to flip direction unpredictably. See R. Bro,
   // "Resolving the Sign Ambiguity in the Singular Value Decomposition".
   // Look ahead to see if the Jacobian's condition will decrease in this
   // direction. Start with a scaled version of the singular vector
-  Eigen::VectorXd delta_x(6);
+  Eigen::VectorXd delta_x(num_dimensions);
   double scale = 100;
   delta_x = vector_toward_singularity / scale;
 
   // Calculate a small change in joints
   Eigen::VectorXd new_theta;
   kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta);
-  new_theta += pseudo_inverse_ * delta_x;
+  new_theta += pseudo_inverse * delta_x;
   kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta);
 
-  jacobian_ = kinematic_state_->getJacobian(joint_model_group_);
-  Eigen::JacobiSVD<Eigen::MatrixXd> new_svd = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian_);
+  Eigen::JacobiSVD<Eigen::MatrixXd> new_svd(jacobian);
   double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1);
   // If new_condition < ini_condition, the singular vector does point towards a
   // singularity. Otherwise, flip its direction.
   if (ini_condition >= new_condition)
   {
-    vector_toward_singularity[0] *= -1;
-    vector_toward_singularity[1] *= -1;
-    vector_toward_singularity[2] *= -1;
-    vector_toward_singularity[3] *= -1;
-    vector_toward_singularity[4] *= -1;
-    vector_toward_singularity[5] *= -1;
+    vector_toward_singularity *= -1;
   }
 
   // If this dot product is positive, we're moving toward singularity ==> decelerate

diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp
@@ -39,13 +39,11 @@
 
 #include "moveit_jog_arm/jog_cpp_interface.h"
 
-// TODO(davetcoleman): rename JogCppApi to JogCppInterface to match file name
-
 static const std::string LOGNAME = "jog_cpp_interface";
 
 namespace moveit_jog_arm
 {
-JogCppApi::JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
+JogCppInterface::JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor)
 {
   planning_scene_monitor_ = planning_scene_monitor;
 
@@ -54,12 +52,12 @@ JogCppApi::JogCppApi(const planning_scene_monitor::PlanningSceneMonitorPtr& plan
     exit(EXIT_FAILURE);
 }
 
-JogCppApi::~JogCppApi()
+JogCppInterface::~JogCppInterface()
 {
   stopMainLoop();
 }
 
-void JogCppApi::startMainLoop()
+void JogCppInterface::startMainLoop()
 {
   // Reset loop termination flag
   stop_requested_ = false;
@@ -150,12 +148,12 @@ void JogCppApi::startMainLoop()
   stopCollisionCheckThread();
 }
 
-void JogCppApi::stopMainLoop()
+void JogCppInterface::stopMainLoop()
 {
   stop_requested_ = true;
 }
 
-void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command)
+void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command)
 {
   shared_variables_mutex_.lock();
 
@@ -183,7 +181,7 @@ void JogCppApi::provideTwistStampedCommand(const geometry_msgs::TwistStamped& ve
   shared_variables_mutex_.unlock();
 };
 
-void JogCppApi::provideJointCommand(const control_msgs::JointJog& joint_command)
+void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_command)
 {
   shared_variables_mutex_.lock();
   shared_variables_.joint_command_deltas = joint_command;
@@ -203,7 +201,7 @@ void JogCppApi::provideJointCommand(const control_msgs::JointJog& joint_command)
   shared_variables_mutex_.unlock();
 }
 
-sensor_msgs::JointState JogCppApi::getJointState()
+sensor_msgs::JointState JogCppInterface::getJointState()
 {
   shared_variables_mutex_.lock();
   sensor_msgs::JointState current_joints = shared_variables_.joints;
@@ -212,7 +210,7 @@ sensor_msgs::JointState JogCppApi::getJointState()
   return current_joints;
 }
 
-bool JogCppApi::getCommandFrameTransform(Eigen::Isometry3d& transform)
+bool JogCppInterface::getCommandFrameTransform(Eigen::Isometry3d& transform)
 {
   if (!jog_calcs_ || !jog_calcs_->isInitialized())
     return false;

diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp
@@ -75,7 +75,7 @@ class TestJogCppInterface : public ::testing::Test
 
 TEST_F(TestJogCppInterface, InitTest)
 {
-  moveit_jog_arm::JogCppApi jog_cpp_interface(planning_scene_monitor_);
+  moveit_jog_arm::JogCppInterface jog_cpp_interface(planning_scene_monitor_);
   ros::Duration(1).sleep();  // Give the started thread some time to run
 }