Jog cpp api fixes #1911

AndyZe · 2020-02-21T18:00:34Z

This cleans up a few things in jog_arm:

Renames JogCppApi class to JogCppInterface to match the filename
Fix a crash when the dimensions of the Jacobian changed
Doxygenize some comments
Make the inheritance of JogInterfaceBase public. I wanted this so I could call the functions from external C++ code, like this: jog_interface_ptr_->changeDriftDimensions(...)

Doxygenize some comments Use public inheritance of JogInterfaceBase

codecov-io · 2020-02-21T19:40:41Z

Codecov Report

Merging #1911 into master will increase coverage by 0.01%.
The diff coverage is 60%.

@@            Coverage Diff            @@
##           master   #1911      +/-   ##
=========================================
+ Coverage   50.29%   50.3%   +0.01%     
=========================================
  Files         313     313              
  Lines       24652   24644       -8     
=========================================
- Hits        12398   12397       -1     
+ Misses      12254   12247       -7

Impacted Files	Coverage Δ
.../moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h	`100% <ø> (ø)`	⬆️
...og_arm/include/moveit_jog_arm/jog_interface_base.h	`100% <ø> (ø)`	⬆️
...erimental/moveit_jog_arm/src/jog_cpp_interface.cpp	`12.04% <37.5%> (ø)`	⬆️
...veit_experimental/moveit_jog_arm/src/jog_calcs.cpp	`72.12% <85.71%> (+1.61%)`	⬆️

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 0abd095...5b1e18a. Read the comment docs.

henningkayser · 2020-02-24T14:19:27Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

@@ -751,17 +750,19 @@ bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::

 void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove)
 {
-  unsigned int num_rows = jacobian.rows() - 1;
-  unsigned int num_cols = jacobian.cols();
+  /*


Why did you remove this?

Oops, it was commented for debugging. Will restore.

henningkayser · 2020-02-24T14:21:25Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

@@ -500,8 +500,7 @@ double JogCalcs::decelerateForSingularity(const Eigen::VectorXd& commanded_veloc
  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 = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian);


Suggested change

Eigen::JacobiSVD<Eigen::MatrixXd> new_svd = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian);

Eigen::JacobiSVD<Eigen::MatrixXd> new_svd(jacobian);

Fix dim mismatch in decelerateForSingularity

jliukkonen · 2020-02-24T18:49:34Z

moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp

@@ -39,13 +39,13 @@

 #include "moveit_jog_arm/jog_cpp_interface.h"

-// TODO(davetcoleman): rename JogCppApi to JogCppInterface to match file name
+// TODO(davetcoleman): rename JogCppInterface to JogCppInterface to match file name


I think this comment can be removed now.

jliukkonen · 2020-02-24T19:00:29Z

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h

@@ -154,8 +154,8 @@ class JogCalcs

  JogArmParameters parameters_;

-  // For jacobian calculations
-  Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_;
+  // Preallocate for jacobian calculations


I would reword comments since the lines below don't really pre-allocate memory but rather cache results that are calculated once(?) Correct me though if I have misunderstood the way Eigen allocates memory for dynamic matrices.

I don't like pre-allocating memory by creating member variables unless there constructor is non-trivial (eg a RobotState variable). It seems like poor design because you never know how they may be used elsewhere and there is no reason why their values should be preserved. Seems like a use case for a static variable instead..

These variables get updated frequently in a while loop (like, 100 times/second or so) so performance kind of matters. I guess I'm not clear how static variables would be better, but I want to know

I'll make them all local variables until I hear back

edit: Oh, I read up on what static does to a local variable (stays in scope until exit). OK, I'll make them static local variables

jliukkonen · 2020-02-24T19:03:07Z

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

@@ -474,23 +474,24 @@ 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,


Could this function be renamed? It calculates a velocity scaling factor but it sounds like it commands the manipulator to decelerate when close to singularity. Maybe velocityScalingFactorForSingularity or something like that.

good idea, done

mlautman · 2020-02-24T21:44:06Z

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h

@@ -154,8 +154,8 @@ class JogCalcs

  JogArmParameters parameters_;

-  // For jacobian calculations
-  Eigen::MatrixXd jacobian_, pseudo_inverse_, matrix_s_;
+  // Preallocate for jacobian calculations


I don't like pre-allocating memory by creating member variables unless there constructor is non-trivial (eg a RobotState variable). It seems like poor design because you never know how they may be used elsewhere and there is no reason why their values should be preserved. Seems like a use case for a static variable instead..

mlautman · 2020-02-24T21:45:08Z

moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h

 * See cpp_interface_example.cpp
 */
-class JogCppApi : protected JogInterfaceBase
+class JogCppInterface : public JogInterfaceBase


Since this is in moveit_experimental in the master branch, this is okay but in general, it is really bad practice to change public interfaces without deprecating the old one.

moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp

* 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

AndyZe added 2 commits February 21, 2020 10:54

Rename JogCppApi->JogCppInterface to match filename

577db29

Doxygenize some comments Use public inheritance of JogInterfaceBase

Fix crash due to different Jacobian dimensions

6268875

AndyZe requested review from henningkayser and tylerjw February 21, 2020 18:00

Clang format, update unit test

5b1e18a

AndyZe requested a review from mlautman February 24, 2020 14:15

henningkayser reviewed Feb 24, 2020

View reviewed changes

Address Henning's comments

de3e985

Fix dim mismatch in decelerateForSingularity

jliukkonen reviewed Feb 24, 2020

View reviewed changes

mlautman reviewed Feb 24, 2020

View reviewed changes

AndyZe added 2 commits February 24, 2020 17:07

Address Mike and Jere's comments

c43746c

Revert 'static' for several variables

00765a1

mlautman approved these changes Feb 26, 2020

View reviewed changes

henningkayser approved these changes Feb 28, 2020

View reviewed changes

henningkayser merged commit 6eaa75d into moveit:master Feb 28, 2020

tylerjw mentioned this pull request May 8, 2020

July 2020 Melodic Release #2066

Closed

20 tasks

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Jog cpp api fixes #1911

Jog cpp api fixes #1911

AndyZe commented Feb 21, 2020

codecov-io commented Feb 21, 2020

henningkayser Feb 24, 2020

AndyZe Feb 24, 2020

henningkayser Feb 24, 2020

AndyZe Feb 24, 2020

jliukkonen Feb 24, 2020

AndyZe Feb 24, 2020

jliukkonen Feb 24, 2020

mlautman Feb 24, 2020

AndyZe Feb 24, 2020 •

edited

Loading

jliukkonen Feb 24, 2020

AndyZe Feb 24, 2020

mlautman Feb 24, 2020

mlautman Feb 24, 2020

	Eigen::JacobiSVD<Eigen::MatrixXd> new_svd = Eigen::JacobiSVD<Eigen::MatrixXd>(jacobian);
	Eigen::JacobiSVD<Eigen::MatrixXd> new_svd(jacobian);

		@@ -474,23 +474,24 @@ 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,

Jog cpp api fixes #1911

Jog cpp api fixes #1911

Conversation

AndyZe commented Feb 21, 2020

codecov-io commented Feb 21, 2020

Codecov Report

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

AndyZe Feb 24, 2020 • edited Loading

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

AndyZe Feb 24, 2020 •

edited

Loading