add Impedance task

debian/libmc-rtc-dev.install

@@ -52,6 +52,8 @@ usr/include/mc_tasks/api.h
 usr/include/mc_tasks/VectorOrientationTask.h
 usr/include/mc_tasks/SplineTrajectoryTask.hpp
 usr/include/mc_tasks/MomentumTask.h
+usr/include/mc_tasks/ImpedanceGains.h
+usr/include/mc_tasks/ImpedanceTask.h
 usr/include/mc_tasks/lipm_stabilizer/*
 usr/include/mc_trajectory/*
 usr/include/mc_planning/*

debian/python3-mc-rtc.install

@@ -1 +1,5 @@
-usr/lib/python3*
+usr/lib/python3/dist-packages/mc_control*
+usr/lib/python3/dist-packages/mc_rbdyn*
+usr/lib/python3/dist-packages/mc_rtc*
+usr/lib/python3/dist-packages/mc_solver*
+usr/lib/python3/dist-packages/mc_tasks*

doc/_data/schemas/ConstraintSet/CompoundJointConstraint.json

@@ -0,0 +1,11 @@
+{
+  "title": "mc_solver::CompoundJointConstraint",
+  "description": "Handle compound joint limits (joints whole limits depend on the position of another joint). By default this adds all the compound joints defined in the robot's module.",
+  "type": "object",
+  "properties":
+  {
+    "type": { "enum": ["compoundJoint"] },
+    "robot": { "$ref": "/../../common/ConstraintSet_robot.json" }
+  },
+ "required": ["type"]
+}

doc/_data/schemas/MetaTask/ImpedanceTask.json

@@ -0,0 +1,39 @@
+{
+  "type": "object",
+  "title": "mc_tasks::force::ImpedanceTask",
+  "description": "This task is similar to AdmittanceTask and DampingTask. Unlike those, the robot can adapt to an external force while following a target pose.",
+  "allOf":
+  [
+    {
+      "$ref": "/../../common/MetaTask_common.json"
+    },
+    {
+      "$ref": "/../../common/MetaTask_surface.json"
+    },
+    {
+      "$ref": "/../../common/SurfaceTransformTask.json"
+    },
+    {
+      "$ref": "/../../common/TrajectoryTaskGeneric.json"
+    },
+    {
+      "properties":
+      {
+        "type": { "enum": ["impedance"] },
+        "weight": { "default": 1000},
+        "stiffness": { "default": 5 },
+        "target": { "$ref" : "/../../SpaceVecAlg/PTransformd.json" },
+        "wrench": { "$ref": "/../../SpaceVecAlg/ForceVecd.json", "description": "<b>Wrench target</b> (desired force-torque)" },
+        "gains": { "$ref": "/../../common/ImpedanceGains.json", "description": "Impedance gains" },
+        "completion": { "$ref": "/../../common/completion_wrench.json" }
+      }
+    },
+    {
+      "properties":
+      {
+        "dimWeight": { "$ref": "/../../Eigen/Vector6d.json" }
+      }
+    }
+  ],
+  "required": ["type", "surface"]
+}

doc/_data/schemas/Observers/BodySensor.json

@@ -6,6 +6,8 @@
   {
     "robot": {"type": "string", "default": "MainRobot", "description": "Name of the robot to observe" },
     "updateRobot": { "type": "string", "default": "<robot>", "description": "Name of the robot to update" },
+    "updatePose": { "type": "boolean", "default": true, "description": "When true updates the real robot base pose" },
+    "updateVel": { "type": "boolean", "default": true, "description": "When true updates the real robot base velocity" },
     "method": { "enum": ["sensor", "control"], "description": "Update from sensor or control values" },
     "bodySensor": { "type": "string", "default": "Main body sensor", "description": "Body sensor used to set the floating base state (only used if method=sensor). When empty, the main body sensor will be used."},
     "log":

doc/_data/schemas/SpaceVecAlg/ImpedanceVecd.json

@@ -0,0 +1,11 @@
+{
+  "title": "sva::ImpedanceVecd",
+  "type": "object",
+  "properties":
+  {
+    "angular": { "$ref": "/../../Eigen/Vector3d.json" },
+    "linear": { "$ref": "/../../Eigen/Vector3d.json" }
+  },
+  "required": ["angular", "linear"],
+  "additionalProperties": false
+}

doc/_data/schemas/common/ImpedanceGains.json

@@ -0,0 +1,12 @@
+{
+  "type": "object",
+  "title": "mc_tasks::force::ImpedanceGains",
+  "description": "Impedance gains configuration for mc_tasks::force::ImpedanceTask",
+  "properties":
+  {
+    "mass": { "$ref": "/../../SpaceVecAlg/ImpedanceVecd.json", "description": "Impedance mass parameter represented in the controlled surface" },
+    "damper": { "$ref": "/../../SpaceVecAlg/ImpedanceVecd.json", "description": "Impedance damper parameter represetnted in the controlled surface" },
+    "spring": { "$ref": "/../../SpaceVecAlg/ImpedanceVecd.json", "description": "Impedance spring parameter repesented in the controlled surface" },
+    "wrench": { "$ref": "/../../SpaceVecAlg/ImpedanceVecd.json", "description": "Coefficient-wise gain on the wrench error" }
+  }
+}

doc/_data/tutorials.yml

@@ -119,6 +119,9 @@
   title: Samples
   desc: This section explains the sample controllers
   tutorials:
+    -
+      id: list-of-samples 
+      title: Available sample controllers 
     -
       id: sample-admittance
       title: Admittance sample controller

doc/_examples/json/ConstraintSet/CompoundJointConstraint.json

@@ -0,0 +1,4 @@
+{
+  "type": "compoundJoint",
+  "robot": "jvrc1"
+}

doc/_examples/json/MetaTask/ImpedanceTask.json

@@ -0,0 +1,21 @@
+{
+  "type": "impedance",
+  "surface": "RightGripper",
+  "target":
+  {
+    // Rotation expressed as RPY (in radian)
+    // May also be expressed as quaternion or rotation matrix
+    "rotation":    [0.0, 0.0, 0.0],
+    "translation": [0.5, 0.5, 1.0]
+  },
+  "wrench":
+  {
+    "couple": [0.0, 0.0, 0.0],
+    "force": [0.0, 0.0, 20.0]
+  },
+  "wrenchGain":
+  {
+    "couple": [0.0, 0.0, 0.0],
+    "force": [0.0, 0.0, 0.0005]
+  }
+}

doc/_examples/json/SpaceVecAlg/ImpedanceVecd.json

@@ -0,0 +1,4 @@
+{
+  "angular": [0.0, 0.0, 0.0],
+  "linear": [0.0, 0.0, 0.0]
+}

doc/_examples/yaml/ConstraintSet/CompoundJointConstraint.yaml

@@ -0,0 +1,3 @@
+---
+type: compoundJoint
+robot: jvrc1

doc/_examples/yaml/MetaTask/ImpedanceTask.yaml

@@ -0,0 +1,13 @@
+type: impedance
+surface: RightGripper
+targetSurface:
+  robot: ground
+  surface: AllGround
+  offset_translation: [0.5, 0.5, 1.2]
+  offset_rotation: [3.14, 0.0, 0.0]
+wrench:
+  couple: [0.0, 0.0, 0.0]
+  force: [0.0, 0.0, 20.0]
+wrenchGain:
+  couple: [0.0, 0.0, 0.0]
+  force: [0.0, 0.0, 0.001]

doc/_examples/yaml/SpaceVecAlg/ImpedanceVecd.yaml

@@ -0,0 +1,2 @@
+angular: [0.0, 0.0, 0.0]
+linear: [0.0, 0.0, 0.0]

doc/tutorials/introduction/configuration.html

@@ -36,7 +36,11 @@
       <td colspan="2">These entries cover most needs you might have</td>
     </tr>
     {% include mc_rtc_configuration_row.html entry="MainRobot" desc="This entry dictates the main robot used by all controllers. Most interface cannot infer the correct module to use based on the simulation environment so this is the user's responsibility to make sure the two match." example="MainRobot: JVRC1" %}
-    {% include mc_rtc_configuration_row.html entry="Enabled" desc="Provides a list of enabled controllers." example="Enabled: [Posture, Body6d, CoM]" %}
+    <tr>
+      <th scope="row">Enabled</th>
+      <td>Provides a list of enabled controllers. See the <a href="{{site.baseurl}}/tutorials/samples/list-of-samples.html">list of all available sample controllers</a>.</td>
+      <td>{% highlight yaml %}Enabled: [Posture, EndEffector, CoM]{% endhighlight %}</td>
+    </tr>
     {% include mc_rtc_configuration_row.html entry="Default" desc="Select which of the enabled controllers will be started first. Note that if the default controller is not enabled or if no default entry is provided then the first enabled controller in the list is chosen as a default controller." example="Default: Posture" %}
     {% include mc_rtc_configuration_row.html entry="Timestep" desc="The controller's timestep." example="Timestep: 0.005" %}
     {% include mc_rtc_configuration_row.html entry="Log" desc="Dictate whether or not controllers will log their output." example="Log: true" %}

doc/tutorials/samples/list-of-samples.md

@@ -0,0 +1,162 @@
+---
+layout: tutorials
+toc: true
+---
+
+In this page you will find a list of all available sample controllers provided with the framework, along with a brief description of how to run them and what to expect. More complex controllers will be further detailed in their own tutorial page.
+
+# Posture
+
+The `Posture` controller (see [online demonstration](https://mc-rtc-demo.netlify.app/#robot=JVRC1&controller=EndEffector)) is the simplest controller in the framework. It adds the following tasks and constraints to the QP solver.
+
+
+
+**Tasks**
+- {% doxygen mc_tasks::PostureTask %}:
+Make each of the robot's DoF target a given joint position. By default this controller will target the current robot encoder values when available, or the default stance for that robot otherwise ({% doxygen mc_rbdyn::RobotModule::stance() %}). Joints can be moved through the joint sliders in the GUI (`Tasks -> posture_jvrc1 -> Target`).
+
+**Constraints**
+- {% doxygen mc_solver::ContactConstraint %}: A controller must always have a contact constraint, but it can have an empty set of contacts.
+- {% doxygen mc_solver::KinematicsConstraint %}: Joint limits constraint.
+- {% doxygen mc_solver::CollisionsConstraint %}: Self-collision avoidance constraint.
+- {% doxygen mc_solver::CompoundJointConstraint %}: Handle joint limits for compound joints (joints whose limits depend on the value of another joint).
+
+**Supported robots**
+All [robots]({{site.baseurl}}/robots.html) supported by the framework. Note that for custom robots, all you need to do is define a default set of collision pairs and limits for the `CompoundJointConstraint` if appropriate (see the tutorial on [integrating a new robot]({{site.baseurl}}/tutorials/advanced/new-robot.html)).
+
+**Running**
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite interface.
+
+```yaml
+MainRobot: JVRC1
+Enabled: Posture
+```
+
+# CoM
+
+The `CoM` sample controller (see [online demonstration](https://mc-rtc-demo.netlify.app/#robot=JVRC1&controller=CoM)) has similar tasks and constraints as the `Posture` controller above.
+
+**Tasks**
+- {% doxygen mc_tasks::PostureTask %}: A posture task with low-weight to provide a default target for all joints.
+- {% doxygen mc_tasks::CoMTask %}: Task controlling the robot's CoM. By default this controller targets the current task position. You can move this target using the interactive markers in the GUI (rviz).
+
+**Constraints**
+- {% doxygen mc_solver::ContactConstraint %}: Adds contacts between the left/right foot and the ground environment.
+- {% doxygen mc_solver::DynamicsConstraint %}: In addition to the joint limits constraint provided by the {% doxygen mc_solver::KinematicsConstraint %} this constraint enables computation of the joint torques and friction cones.
+- {% doxygen mc_solver::CollisionsConstraint %}: Self-collision avoidance constraint.
+- {% doxygen mc_solver::CompoundJointConstraint %}: Handle joint limits for compound joints (joints whose limits depend on the value of another joint).
+
+**Supported robots**
+All biped [robots]({{site.baseurl}}/robots.html) supported by the framework. Note that for custom robots, all you need to do is define a `LeftFoot` and `RightFoot` surface in [your robot description]({{site.baseurl}}/tutorials/advanced/new-robot.html).
+
+
+**Running**
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite interface.
+
+```yaml
+MainRobot: JVRC1
+Enabled: CoM
+```
+
+# EndEffector
+
+The `EndEffector` controller (see [online demonstration](https://mc-rtc-demo.netlify.app/#robot=JVRC1&controller=EndEffector)) has the same tasks and constraints as the `CoM` controller above. In addition to controlling the CoM position, it adds an {% doxygen mc_tasks::EndEffectorTask %} to control the position and orientation of the robot's hand end-effector.
+
+**Supported robots**
+All biped [robots]({{site.baseurl}}/robots.html) supported by the framework. Note that for custom robots, all you need to do is define a `LeftFoot` and `RightFoot` surfaces and an `r_wrist` body in [your robot]({{site.baseurl}}/tutorials/advanced/new-robot.html).
+
+**Running**
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite interface.
+
+```yaml
+MainRobot: JVRC1
+Enabled: EndEffector
+```
+
+# Text
+
+The `Text` sample controller demonstrate use of the {% doxygen mc_tasks::MetaTasks %} and {% doxygen mc_solver::ConstraintSetLoader %} to load [tasks]({{site.baseurl}}/json.html#MetaTask}} and [constraints]({{site.baseurl}}/json.html#ConstraintSet) from their YAML configuration.
+
+**Running**
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite interface.
+
+```yaml
+MainRobot: JVRC1
+Enabled: Text
+```
+
+The default configuration for this controller is similar to the `EndEffector` controller. You can try writing your own configuration in `~/.config/mc_rtc/controllers/Text.yaml`. For example, the following configuration re-creates the `CoM` controller and moves the `CoM` above the `LeftFoot` surface:
+
+```yaml
+Text:
+  constraints:
+  - type: dynamics
+  - type: contact
+  - type: collision
+  - type: compoundJoint
+  tasks:
+  - type: com
+    weight: 1000
+    above: [LeftFoot]
+  contacts:
+  - r1Surface: LeftFoot
+    r2Surface: AllGround
+    isFixed: false
+  - r1Surface: RightFoot
+    r2Surface: AllGround
+    isFixed: false
+```
+
+# Admittance
+
+The `Admittance` sample controller demonstrates the use of the {% doxygen mc_tasks::force::AdmittanceTask %} though a simple FSM making the `JVRC1` robot push a wall with a desired force.
+
+**Detailed description**: see the [Admittance Sample Controller tutorial]({{site.baseurl}}/tutorials/samples/sample-admittance.html)
+
+**Supported robots**: `JVRC1`.
+
+**Prerequisites**: Please make sure that the hand force sensors have been calibrated beforehand using the [ForceSensorCalibration controller](https://github.com/jrl-umi3218/mc_force_sensor_calibration_controller).
+
+**Running**
+
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite dynamic simulation (e.g Choreonoid...).
+
+```yaml
+MainRobot: JVRC1
+Enabled: Admittance
+```
+
+# Impedance
+
+The `Impedance` sample controller demonstrates the use of the {% doxygen mc_tasks::force::ImpedanceTask %} to simulataneously perform position and force control of the robot's end-effector.
+
+**Supported robots**: `JVRC1Fixed`.
+
+**Prerequisites**: Please make sure that the hand force sensors have been calibrated beforehand using the [ForceSensorCalibration controller](https://github.com/jrl-umi3218/mc_force_sensor_calibration_controller).
+
+**Running**
+
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite dynamic simulation (e.g Choreonoid...). In choreonoid you can apply external forces by clicking on the moving hand and dragging it. It will follow you to some extent and return to it's expected position along the trajectory when you relase the force. Note that you may add additional {% doxygen mc_tasks::force::ImpedanceTask %} to the free end-effectors throught the GUI (`Global  -> Add Tasks -> ImpedanceTask`).
+
+```yaml
+MainRobot: JVRC1Fixed
+Enabled: Impedance
+```
+
+# LIPMStabilizer
+
+The `LIPMStabilizer` sample controller demonstrates the use of the {% doxygen mc_tasks::StabilizerTask %} and its corresponding convenience FSM state {% doxygen mc_state::StabilizerStandingState %} though a simple quasi-static FSM making the robot stand and step while keeping balance.
+
+**Detailed description**: see [the LIPM Stabilizer tutorial]({{site.baseurl}}/tutorials/recipes/lipm-stabilizer.html)
+
+**Supported robots**: All biped [robots]({{site.baseurl}}/robots.html) supported by the framework.
+
+**Running**
+
+To run this controller, simply put in [your mc_rtc configuration]({{site.baseurl}}/tutorials/introduction/configuration.html), and [run the controller]({{site.baseurl}}/tutorials/introduction/running-a-controller.html) using your favorite dynamic simulation (e.g Choreonoid...).
+
+```yaml
+MainRobot: JVRC1
+Enabled: LIPMStabilizer
+```
+

include/mc_filter/LowPass.h

@@ -33,9 +33,7 @@ struct LowPass
     reset(T::Zero());
   }
 
-  /** Get cutoff period.
-   *
-   */
+  /** Get cutoff period. */
   double cutoffPeriod() const
   {
     return cutoffPeriod_;
@@ -87,11 +85,27 @@ struct LowPass
     return eval_;
   }
 
+  /** Get sampling period.
+   *
+   */
   double dt() const
   {
     return dt_;
   }
 
+  /** Set sampling period.
+   *
+   * \param dt Sampling period.
+   *
+   * \note the cutoff period is updated to satisfy the Nyquist–Shannon sampling theorem according the new sampling
+   * period.
+   */
+  void dt(double dt)
+  {
+    dt_ = dt;
+    cutoffPeriod(cutoffPeriod_);
+  }
+
 private:
   T eval_;
   double cutoffPeriod_ = 0.;

include/mc_observers/BodySensorObserver.h

@@ -94,6 +94,8 @@ struct MC_OBSERVER_DLLAPI BodySensorObserver : public Observer
   sva::MotionVecd accW_ = sva::MotionVecd::Zero();
   std::string robot_;
   std::string updateRobot_;
+  bool updatePose_ = true;
+  bool updateVel_ = true;
 
   bool logPos_ = true;
   bool logVel_ = true;

include/mc_rtc/log/utils.h

@@ -84,6 +84,7 @@ IMPL_MAPPING(Eigen::Quaterniond, Quaterniond);
 IMPL_MAPPING(sva::PTransformd, PTransformd);
 IMPL_MAPPING(sva::ForceVecd, ForceVecd);
 IMPL_MAPPING(sva::MotionVecd, MotionVecd);
+IMPL_MAPPING(sva::ImpedanceVecd, MotionVecd);
 
 #undef IMPL_MAPPING