chore: use current pose as nullspace target in cartesian motion gener…

…ation (#36)
JeanElsner · Jul 9, 2024 · 9580bda · 9580bda
1 parent 41e6b8f
commit 9580bda
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 3 deletions.
diff --git a/include/controllers/cartesian_trajectory.h b/include/controllers/cartesian_trajectory.h
@@ -11,6 +11,7 @@ class CartesianTrajectory : public CartesianImpedance {
   static const Eigen::Matrix<double, 6, 6> kDefaultImpedance;
 
   CartesianTrajectory(std::shared_ptr<motion::CartesianTrajectory> trajectory,
+             const Vector7d &q_init,
              const Eigen::Matrix<double, 6, 6> &impedance = kDefaultImpedance,
              const double &damping_ratio = kDefaultDampingRatio,
              const double &nullspace_stiffness = kDefaultNullspaceStiffness,
@@ -24,6 +25,7 @@ class CartesianTrajectory : public CartesianImpedance {
 
  private:
   std::shared_ptr<motion::CartesianTrajectory> traj_;
+  Vector7d q_init_;
   double dq_threshold_;
 };
 

diff --git a/src/controllers/cartesian_trajectory.cpp b/src/controllers/cartesian_trajectory.cpp
@@ -16,20 +16,22 @@ const Eigen::Matrix<double, 6, 6> CartesianTrajectory::kDefaultImpedance =
     Eigen::Matrix<double, 6, 6>(_data);
 
 CartesianTrajectory::CartesianTrajectory(std::shared_ptr<motion::CartesianTrajectory> trajectory,
+             const Vector7d &q_init,
              const Eigen::Matrix<double, 6, 6> &impedance,
              const double &damping_ratio,
              const double &nullspace_stiffness,
              const double dq_threshold,
              const double filter_coeff)
     : CartesianImpedance(impedance, damping_ratio, nullspace_stiffness, filter_coeff),
       traj_(trajectory),
-      dq_threshold_(dq_threshold) {}
+      dq_threshold_(dq_threshold),
+      q_init_(q_init) {}
 
 franka::Torques CartesianTrajectory::step(const franka::RobotState &robot_state,
                                  franka::Duration &duration) {
   auto position = traj_->getPosition(getTime());
   auto orientation = traj_->getOrientation(getTime());
-  setControl(position, orientation);
+  setControl(position, orientation, q_init_);
   auto torques = CartesianImpedance::step(robot_state, duration);
   if (getTime() > traj_->getDuration()) {
     bool at_rest = true;

diff --git a/src/panda.cpp b/src/panda.cpp
@@ -379,7 +379,7 @@ bool Panda::moveToPose(std::vector<Eigen::Vector3d> &positions,
     return true;
   }
   auto ctrl = std::make_shared<controllers::CartesianTrajectory>(
-      traj, impedance, damping_ratio, nullspace_stiffness, dq_threshold, 1.0);
+      traj, getJointPositions(), impedance, damping_ratio, nullspace_stiffness, dq_threshold, 1.0);
   _startController(ctrl);
   auto cb = _createTorqueCallback();
   _runController(cb);