Feature: End-Effector Workspace Pose and Velocity (Linear and Angular) Factor #24
Conversation
| if (H1) { | ||
| // Jacobian for the joint positions | ||
| *H1 = (Matrix(12, arm_.dof()) << H_ep * J_jpx_jp[arm_.dof() - 1],// | ||
| - J_jvx_jp[arm_.dof() - 1]).finished(); |
There was a problem hiding this comment.
Some weird formatting issue is happening here. Can you please reformat the file?
| std::cout << s << "WorkspacePoseVelocityPrior :" << std::endl; | ||
| Base::print("", keyFormatter); | ||
| std::cout << "desired end-effector pose : "; | ||
| des_pose_.print(); // << std::endl; |
There was a problem hiding this comment.
Kill the comment if the std::endl is not being used.
| template <class ARCHIVE> | ||
| void serialize(ARCHIVE& ar, const unsigned int version) { | ||
| ar& boost::serialization::make_nvp( | ||
| "NoiseModelFactor1", boost::serialization::base_object<Base>(*this)); |
There was a problem hiding this comment.
I think this should NoiseModelFactor2 and not NoiseModelFactor1
| WorkspacePoseVelocityPrior() {} | ||
|
|
||
| /** | ||
| * Constructor |
There was a problem hiding this comment.
Please add documentation for all parameters.
| gtsam::DefaultKeyFormatter) const { | ||
| std::cout << s << "WorkspacePoseVelocityPrior :" << std::endl; | ||
| Base::print("", keyFormatter); | ||
| std::cout << "desired end-effector pose : "; |
There was a problem hiding this comment.
Nitpick: capitalize the first letter so the output looks prettier?
Similarly for the below output of the end effector velocity.
| Vector6 des_vel = (Vector6() << 0, 0, 0, 0, 0, 0).finished(); | ||
| WorkspacePoseVelocityPrior factor(0, 0, cost_model, arm, des_pose, des_vel); | ||
| actual = factor.evaluateError(q, qdot, &H_pose_act, &H_vel_act); | ||
| expect = (Vector(12) << 00.613943126, 1.48218982, -0.613943126, 1.1609828, |
There was a problem hiding this comment.
Can you please comment where you got these numbers from? And please mark this as a regression test.
| 0.706727485, -0.547039678, 0, -0.141421356237309, 0, | ||
| -0.070710678118655, 0.070710678118655, -0.1000) | ||
| .finished(); | ||
| H_pose_exp = numericalDerivative11( |
There was a problem hiding this comment.
Why not use numericalDerivative21 and numericalDerivative22? It computes the numerical derivative of a binary function wrt the first and second argument respectively.
| qdot, 1e-6); | ||
| EXPECT(assert_equal(expect, actual, 1e-6)); | ||
| EXPECT(assert_equal(H_pose_exp, H_pose_act, 1e-6)); | ||
| EXPECT(assert_equal(H_vel_exp, H_vel_act, 1e-6)); |
There was a problem hiding this comment.
Assertions should follow immediately after the computation so that the code can fail early without doing extra computation. In this case, the first assert_equal should be right after the expect variable definition.
Purpose
This factor enables users to specify a desired SE(3) pose and the corresponding linear and angular velocity to the end-effector of a manipulator.
Add Features