Ft remove oscilations

CMakeLists.txt

@@ -24,6 +24,7 @@ pybind11_add_module(_core
   src/controllers/joint_limits/virtual_wall.cpp
   src/controllers/integrated_velocity.cpp
   src/controllers/joint_position.cpp
+  src/controllers/joint_velocity.cpp
   src/controllers/cartesian_impedance.cpp
   src/controllers/applied_torque.cpp
   src/controllers/applied_force.cpp

examples/joint_velocity.py

@@ -0,0 +1,42 @@
+import sys
+import typing
+
+import numpy as np
+
+import panda_py
+from panda_py import controllers
+
+_Shape = typing.Tuple[int, ...]
+
+if __name__ == '__main__':
+    if len(sys.argv) < 2:
+        raise RuntimeError(f'Usage: python {sys.argv[0]} <robot-hostname>')
+
+    stiffness = np.array([600., 600., 600., 600., 250., 150., 50.]) / 10
+
+    ctrl = controllers.JointVelocity(stiffness=stiffness)
+    panda = panda_py.Panda(sys.argv[1])
+    q_0 = panda.get_state().q
+    panda.move_to_start()
+    panda.start_controller(ctrl)
+    runtime = 50
+    qdot_des = np.zeros(7)
+
+    joint_positions = []
+    joint_velocities = []
+
+
+
+    with panda.create_context(frequency=100, max_runtime=runtime) as ctx:
+        while ctx.ok():
+            #qdot_des[0] += 0.01 * np.cos(ctrl.get_time())
+            state = panda.get_state()
+            q = state.q
+            dq = state.dq
+            qdot_des = 0.3 * np.cos(ctrl.get_time()) * np.ones(7)
+            ctrl.set_control(q, qdot_des)
+            joint_positions.append(q)
+            joint_velocities.append(dq)
+
+    np.save('q', np.array(joint_positions))
+    np.save('q_d', np.array(joint_velocities))

include/controllers/joint_velocity.h

@@ -0,0 +1,46 @@
+#pragma once
+#include <atomic>
+#include <mutex>
+
+#include "controllers/controller.h"
+#include "utils.h"
+
+class JointVelocity : public TorqueController {
+ public:
+  static const Vector7d kDefaultStiffness;
+  static const Vector7d kDefaultDamping;
+  static const Vector7d kDefaultDqd;
+  static const Vector7d kDefaultI;
+  static const Vector7d velErrorCumMaxDefault;
+  static const Vector7d velErrorCumMinDefault;
+  static const double kDefaultFilterCoeff;
+
+  JointVelocity(const Vector7d &stiffness = kDefaultStiffness,
+                const Vector7d &damping = kDefaultDamping,
+                const double filter_coeff = kDefaultFilterCoeff,
+                const Vector7d &vel_error_cum_max= velErrorCumMaxDefault,
+                const Vector7d &vel_error_cum_min= velErrorCumMinDefault,
+                const Vector7d &k_i = kDefaultI);
+
+  franka::Torques step(const franka::RobotState &robot_state,
+                       franka::Duration &duration) override;
+  void setControl(const Vector7d &velocity);
+  void setStiffness(const Vector7d &stiffness);
+  void setDamping(const Vector7d &damping);
+  void setVelErrorCumMax(const Vector7d &vel_error_cum_max);
+  void setVelErrorCumMin(const Vector7d &vel_error_cum_min);
+  void setKi(const Vector7d &K_i);
+  void setFilter(const double filter_coeff);
+  void start(const franka::RobotState &robot_state, std::shared_ptr<franka::Model> model) override;
+  void stop(const franka::RobotState &robot_state, std::shared_ptr<franka::Model> model) override;
+  bool isRunning() override;
+  const std::string name() override;
+
+ private:
+  Vector7d K_p_, K_d_, q_d_, dq_d_, dq_filtered, K_p_target_, K_d_target_, q_d_target_, dq_d_target_, K_i_, vel_error_cum_max_, vel_error_cum_min_, vel_error, vel_error_cum;
+  double filter_coeff_;
+  std::mutex mux_;
+  std::atomic<bool> motion_finished_;
+
+  void _updateFilter();
+};

pyproject.toml

@@ -7,7 +7,7 @@ name = "panda-python"
 description = "Python bindings for the Panda robot"
 requires-python = ">=3.7"
 dependencies = ["websockets>=11.0", "requests", "numpy"]
-version = "0.6.0"
+version = "0.6.1"
 authors = [
     { name = "Jean Elsner", email = "[email protected]" },
 ]

src/_core.cpp

@@ -11,6 +11,7 @@
 #include "controllers/force.h"
 #include "controllers/integrated_velocity.h"
 #include "controllers/joint_position.h"
+#include "controllers/joint_velocity.h"
 #include "kinematics/fk.h"
 #include "kinematics/ik.h"
 #include "motion/generators.h"
@@ -318,6 +319,28 @@ PYBIND11_MODULE(_core, m) {
       .def("set_filter", &JointPosition::setFilter,
            py::call_guard<py::gil_scoped_release>(), py::arg("filter_coeff"));
 
+  py::class_<JointVelocity, TorqueController, std::shared_ptr<JointVelocity>>(
+      m, "JointVelocity")
+      .def(py::init<const Vector7d &, const Vector7d &,
+                    const double>(), /*py::keep_alive<1, 0>(),*/
+           py::arg("stiffness") = JointVelocity::kDefaultStiffness,
+           py::arg("damping") = JointVelocity::kDefaultDamping,
+           py::arg("filter_coeff") = JointVelocity::kDefaultFilterCoeff)
+      .def("set_control", &JointVelocity::setControl,
+           py::call_guard<py::gil_scoped_release>(), py::arg("velocity"))
+      .def("set_stiffness", &JointVelocity::setStiffness,
+           py::call_guard<py::gil_scoped_release>(), py::arg("stiffness"))
+      .def("set_ki", &JointVelocity::setKi,
+           py::call_guard<py::gil_scoped_release>(), py::arg("k_i"))
+      .def("set_vel_error_cum_max", &JointVelocity::setVelErrorCumMax,
+           py::call_guard<py::gil_scoped_release>(), py::arg("vel_error_cum_max"))
+      .def("set_vel_error_cum_min", &JointVelocity::setVelErrorCumMin,
+           py::call_guard<py::gil_scoped_release>(), py::arg("vel_error_cum_min"))
+      .def("set_damping", &JointVelocity::setDamping,
+           py::call_guard<py::gil_scoped_release>(), py::arg("damping"))
+      .def("set_filter", &JointVelocity::setFilter,
+           py::call_guard<py::gil_scoped_release>(), py::arg("filter_coeff"));
+
   py::class_<CartesianImpedance, TorqueController,
              std::shared_ptr<CartesianImpedance>>(m, "CartesianImpedance")
       .def(py::init<const Eigen::Matrix<double, 6, 6> &, const double &,

src/controllers/joint_velocity.cpp

@@ -0,0 +1,130 @@
+#include "controllers/joint_velocity.h"
+
+#include <iostream>
+
+#include "panda.h"
+
+const double kDefaultStiffnessData[7] = {600, 600, 600, 600, 250, 150, 50};
+const Vector7d JointVelocity::kDefaultStiffness =
+    Vector7d(kDefaultStiffnessData);
+
+const double kDefaultDqdData[7] = {0, 0, 0, 0, 0, 0, 0};
+const Vector7d JointVelocity::kDefaultDqd = Vector7d(kDefaultDqdData);
+
+const double kDefaultDampingData[7] = {50, 50, 50, 20, 20, 20, 10};
+const Vector7d JointVelocity::kDefaultDamping = Vector7d(kDefaultDampingData);
+const double JointVelocity::kDefaultFilterCoeff = 1.0;
+
+const double kDefaultIData[7] = {5, 5, 5, 2, 2, 2, 1};
+const Vector7d JointVelocity::kDefaultI = Vector7d(kDefaultIData);
+
+const double velErrorCumMaxDefaultData[7] = {0.5, 0.5, 0.5, 0.2, 0.2, 0.2, 0.1};
+const Vector7d JointVelocity::velErrorCumMaxDefault = Vector7d(velErrorCumMaxDefaultData);
+
+const double velErrorCumMinDefaultData[7] = {-0.5, -0.5, -0.5, -0.2, -0.2, -0.2, -0.1};
+const Vector7d JointVelocity::velErrorCumMinDefault = Vector7d(velErrorCumMinDefaultData);
+
+JointVelocity::JointVelocity(const Vector7d &stiffness, const Vector7d &damping,
+                             const double filter_coeff,
+                             const Vector7d &vel_error_cum_max,
+                             const Vector7d &vel_error_cum_min,
+                             const Vector7d &k_i) {
+  K_p_ = stiffness;
+  K_p_target_ = stiffness;
+  K_d_ = damping;
+  K_d_target_ = damping;
+  K_i_ = k_i;
+  vel_error_cum_max_ = vel_error_cum_max;
+  vel_error_cum_min_ = vel_error_cum_min;
+  filter_coeff_ = filter_coeff;
+};
+
+franka::Torques JointVelocity::step(const franka::RobotState &robot_state,
+                                    franka::Duration &duration) {
+  // Observed states
+  Vector7d q, q_d, dq_d, dq, tau_d, K_p, K_d;
+  q = Eigen::Map<const Vector7d>(robot_state.q.data());
+  dq = Eigen::Map<const Vector7d>(robot_state.dq.data());
+  // These quantities may be modified outside of the control loop
+  mux_.lock();
+  _updateFilter(); // Filter desired reference
+
+  // Filter velocity readings
+  dq_filtered = ema_filter(dq_filtered, dq, filter_coeff_, true);
+
+  K_p = K_p_;
+  K_d = K_d_;
+  q_d = q_d_;
+  dq_d = dq_d_;
+  mux_.unlock();
+  vel_error = (dq_d - dq_filtered);
+  vel_error_cum += vel_error;
+  vel_error_cum = vel_error_cum.cwiseMax(vel_error_cum_min_).cwiseMin(vel_error_cum_max_);
+  // PD control
+  //tau_d << K_p.asDiagonal() * (q_d - q) + K_d.asDiagonal() * (dq_d - dq);
+  tau_d << K_p.asDiagonal() * vel_error + K_i_.asDiagonal() * vel_error_cum - K_d.asDiagonal() * dq_filtered;
+  franka::Torques torques = VectorToArray(tau_d);
+  torques.motion_finished = motion_finished_;
+  return torques;
+}
+
+void JointVelocity::_updateFilter() {
+  q_d_ = ema_filter(q_d_, q_d_target_, filter_coeff_, true);
+  dq_d_ = ema_filter(dq_d_, dq_d_target_, filter_coeff_, true);
+  K_p_ = ema_filter(K_p_, K_p_target_, filter_coeff_, true);
+  K_d_ = ema_filter(K_d_, K_d_target_, filter_coeff_, true);
+}
+
+void JointVelocity::setControl(const Vector7d &velocity) {
+  std::lock_guard<std::mutex> lock(mux_);
+  dq_d_target_ = velocity;
+}
+
+void JointVelocity::setStiffness(const Vector7d &stiffness) {
+  std::lock_guard<std::mutex> lock(mux_);
+  K_p_target_ = stiffness;
+}
+
+void JointVelocity::setDamping(const Vector7d &damping) {
+  std::lock_guard<std::mutex> lock(mux_);
+  K_d_target_ = damping;
+}
+
+void JointVelocity::setKi(const Vector7d &K_i) {
+  std::lock_guard<std::mutex> lock(mux_);
+  K_i_ = K_i;
+}
+
+void JointVelocity::setVelErrorCumMax(const Vector7d &vel_error_cum_max) {
+  std::lock_guard<std::mutex> lock(mux_);
+  vel_error_cum_max_ = vel_error_cum_max;
+}
+
+void JointVelocity::setVelErrorCumMin(const Vector7d &vel_error_cum_min) {
+  std::lock_guard<std::mutex> lock(mux_);
+  vel_error_cum_min_ = vel_error_cum_min;
+}
+
+void JointVelocity::setFilter(const double filter_coeff) {
+  std::lock_guard<std::mutex> lock(mux_);
+  filter_coeff_ = filter_coeff;
+}
+
+void JointVelocity::start(const franka::RobotState &robot_state,
+                          std::shared_ptr<franka::Model> model) {
+  motion_finished_ = false;
+  q_d_ = Eigen::Map<const Vector7d>(robot_state.q.data());
+  q_d_target_ = Eigen::Map<const Vector7d>(robot_state.q.data());
+  dq_d_.setZero();
+  dq_filtered.setZero();
+  dq_d_target_.setZero();
+}
+
+void JointVelocity::stop(const franka::RobotState &robot_state,
+                         std::shared_ptr<franka::Model> model) {
+  motion_finished_ = true;
+}
+
+bool JointVelocity::isRunning() { return !motion_finished_; }
+
+const std::string JointVelocity::name() { return "Joint Position"; }

src/panda_py/_core/__init__.pyi

@@ -13,6 +13,7 @@ __all__ = [
     "Force",
     "IntegratedVelocity",
     "JointPosition",
+    "JointVelocity",
     "JointTrajectory",
     "Panda",
     "PandaContext",
@@ -95,6 +96,16 @@ class JointPosition(TorqueController):
     def set_filter(self, filter_coeff: float) -> None: ...
     def set_stiffness(self, stiffness: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
     pass
+class JointVelocity(TorqueController):
+    def __init__(self, stiffness: numpy.ndarray[numpy.float64, _Shape[7, 1]] = array([600., 600., 600., 600., 250., 150., 50.]), damping: numpy.ndarray[numpy.float64, _Shape[7, 1]] = array([50., 50., 50., 20., 20., 20., 10.]), filter_coeff: float = 1.0) -> None: ...
+    def set_control(self, position: numpy.ndarray[numpy.float64, _Shape[7, 1]], velocity: numpy.ndarray[numpy.float64, _Shape[7, 1]] = array([0., 0., 0., 0., 0., 0., 0.])) -> None: ...
+    def set_damping(self, damping: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
+    def set_filter(self, filter_coeff: float) -> None: ...
+    def set_stiffness(self, stiffness: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
+    def set_ki(self, ki: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
+    def set_vel_error_cum_max(self, vel_error_cum_max: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
+    def set_vel_error_cum_min(self, vel_error_cum_min: numpy.ndarray[numpy.float64, _Shape[7, 1]]) -> None: ...
+    pass
 class JointTrajectory():
     def __init__(self, waypoints: typing.List[numpy.ndarray[numpy.float64, _Shape[7, 1]]], speed_factor: float = 0.2, max_deviation: float = 0, timeout: float = 30.0) -> None: ...
     def get_duration(self) -> float: ...

src/panda_py/controllers.py

@@ -8,9 +8,9 @@
 # pylint: disable=no-name-in-module
 from ._core import AppliedForce, AppliedTorque,\
                     CartesianImpedance, Force, IntegratedVelocity,\
-                    JointPosition, TorqueController
+                    JointPosition, JointVelocity, TorqueController
 
 __all__ = [
     'TorqueController', 'CartesianImpedance', 'IntegratedVelocity',
-    'JointPosition', 'AppliedTorque', 'AppliedForce', 'Force'
+    'JointPosition', 'JointVelocity', 'AppliedTorque', 'AppliedForce', 'Force'
 ]