Mobile arm controller (sources)

Note: api.h, CMakeLists.txt and src/CMakeLists.txt are the same as in the previous example

MobileArmController.h

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

#pragma once

#include <mc_control/mc_controller.h>
#include <mc_tasks/SurfaceTransformTask.h>
#include <mc_tasks/TransformTask.h>

#include "api.h"

enum Phase
{
  APPROACH = 0,
  HANDLE,
  OPEN,
  DONE
};

struct MobileArmController_DLLAPI MobileArmController : public mc_control::MCController
{
public:
  MobileArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config);
  bool run() override;
  void reset(const mc_control::ControllerResetData & reset_data) override;

private:
  Phase phase_ = APPROACH;
  std::shared_ptr<mc_tasks::SurfaceTransformTask> handTask_;
  std::shared_ptr<mc_tasks::TransformTask> dingoBaseTask_;
  std::shared_ptr<mc_solver::KinematicsConstraint> doorKinematics_;
  std::shared_ptr<mc_tasks::PostureTask> doorPostureTask_;
};

MobileArmController.cpp

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87

#include "MobileArmController.h"

#include <mc_rbdyn/RobotLoader.h>
#include <mc_tasks/TransformTask.h>
#include <chrono>
#include <thread>

MobileArmController::MobileArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController({rm, mc_rbdyn::RobotLoader::get_robot_module("dingo"),
                            mc_rbdyn::RobotLoader::get_robot_module("env/door"),
                            mc_rbdyn::RobotLoader::get_robot_module("env/ground")},
                           dt)
{
  solver().addConstraintSet(contactConstraint);
  solver().addConstraintSet(selfCollisionConstraint);
  solver().addTask(postureTask);
  solver().setContacts({{}});

  Eigen::Vector6d dof = Eigen::Vector6d::Ones();
  double friction = mc_rbdyn::Contact::defaultFriction;
  dof[2] = 0.0;
  dof[3] = 0.0;
  dof[4] = 0.0;
  addContact({"dingo", "ground", "Base", "AllGround", friction, dof});

  double iDist = 0.1;
  double sDist = 0.05;
  addCollisions("dingo", "door", {{"*", "*", iDist, sDist, 0}});
  addCollisions("ur5e", "door", {{"*", "*", iDist, sDist, 0}});

  mc_rtc::log::success("MobileArmController init done ");
}

bool MobileArmController::run()
{
  if(phase_ == APPROACH && dingoBaseTask_->eval().norm() < 1e-2 && dingoBaseTask_->speed().norm() < 1e-3)
  {
    solver().removeTask(postureTask);
    handTask_->reset();
    solver().addTask(handTask_);
    handTask_->target(sva::PTransformd(Eigen::Vector3d(0, 0, -0.05)) * robots().robot(2).surfacePose("Handle"));
    phase_ = HANDLE;
  }
  else if(phase_ == HANDLE && handTask_->eval().norm() < 0.1 && handTask_->speed().norm() < 1e-4)
  {
    addContact({"ur5e", "door", "Tool", "Handle"});
    solver().removeTask(handTask_);
    doorPostureTask_->target({{"handle", {-1.0}}});
    phase_ = OPEN;
  }
  else if(phase_ == OPEN && doorPostureTask_->eval().norm() < 0.01)
  {
    solver().removeTask(dingoBaseTask_);
    doorPostureTask_->target({{"door", {M_PI / 2}}});
    phase_ = DONE;
  }
  else if(phase_ == DONE && doorPostureTask_->eval().norm() < 0.01)
  {
    removeContact({"ur5e", "door", "Tool", "Handle"});
    solver().addTask(dingoBaseTask_);
    solver().addTask(postureTask);
  }
  return mc_control::MCController::run();
}

void MobileArmController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);
  robots().robot(0).posW(sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(0.0, 0.0, 0.5)));
  robots().robot(1).posW(sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(-0.25, 0.0, 0.0)));
  robots().robot(2).posW(sva::PTransformd(sva::RotZ(M_PI), Eigen::Vector3d(2.0, 1.0, 0)));
  addContact({"dingo", "ur5e", "Base", "Base"});

  handTask_ = std::make_shared<mc_tasks::SurfaceTransformTask>("Tool", robots(), 0);
  dingoBaseTask_ = std::make_shared<mc_tasks::TransformTask>("base_link", robots(), 1, 2.0, 1000);

  doorKinematics_ = std::make_shared<mc_solver::KinematicsConstraint>(robots(), 2, solver().dt());
  solver().addConstraintSet(*doorKinematics_);
  doorPostureTask_ = std::make_shared<mc_tasks::PostureTask>(solver(), 2, 1.0, 1.0);
  solver().addTask(doorPostureTask_);

  solver().addTask(dingoBaseTask_);
  postureTask->target({{"shoulder_lift_joint", {-M_PI / 2}}});
  dingoBaseTask_->target({Eigen::Vector3d(1.5, 0.0, 0.0)});
}

CONTROLLER_CONSTRUCTOR("MobileArmController", MobileArmController)

dual_arm_controller.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136

import os
import sva
import math
import eigen
import mc_tasks
import mc_rbdyn
import mc_solver
import mc_control


class ControllerPhase:
    APPROACH = 0
    HANDLE = 1
    OPEN = 2
    DONE = 3


class MobileArmController(mc_control.MCPythonController):
    def __init__(self, rm, dt):
        self.qpsolver.addConstraintSet(self.contactConstraint)
        self.qpsolver.addConstraintSet(self.selfCollisionConstraint)
        self.qpsolver.addTask(self.postureTask)

        dof = eigen.Vector6d.Zero()
        dof[0] = 1.0
        dof[1] = 1.0
        dof[5] = 1.0
        friction = mc_rbdyn.Contact.defaultFriction
        self.addContact(
            mc_control.Contact("dingo", "ground", "Base", "AllGround", friction, dof)
        )

        iDist, sDist, damping = 0.1, 0.05, 0.0
        self.addCollisions(
            "dingo",
            "door",
            [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
        )
        self.addCollisions(
            "ur5e",
            "door",
            [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
        )

        self._phase = ControllerPhase.APPROACH

    def run_callback(self):
        if (
            self._phase == ControllerPhase.APPROACH
            and self._dingoBaseTask.eval().norm() < 1e-2
            and self._dingoBaseTask.speed().norm() < 1e-5
        ):
            self.qpsolver.removeTask(self.postureTask)
            self._handTask.reset()
            self.qpsolver.addTask(self._handTask)
            self._handTask.target(
                sva.PTransformd(eigen.Vector3d(0.0, 0.0, -0.05))
                * self.robots().robot(2).surfacePose("Handle")
            )
            self._phase = ControllerPhase.HANDLE
        elif (
            self._phase == ControllerPhase.HANDLE
            and self._handTask.eval().norm() < 0.1
            and self._handTask.speed().norm() < 1e-4
        ):
            self.addContact(mc_control.Contact("ur5e", "door", "Tool", "Handle"))
            self.qpsolver.removeTask(self._handTask)
            self.postureTask.reset()
            self._doorPosture.target(
                {
                    "handle".encode("utf-8"): [-1.0],
                }
            )
            self._phase = ControllerPhase.OPEN
        elif (
            self._phase == ControllerPhase.OPEN
            and self._doorPosture.eval().norm() < 0.01
        ):
            self.qpsolver.removeTask(self._dingoBaseTask)
            self._doorPosture.target(
                {
                    "door".encode("utf-8"): [math.pi / 2],
                }
            )
            self._phase = ControllerPhase.DONE
        elif (
            self._phase == ControllerPhase.DONE
            and self._doorPosture.eval().norm() < 0.01
        ):
            self.removeContact(mc_control.Contact("ur5e", "door", "Tool", "Handle"))
            self._dingoBaseTask.reset()
            self.qpsolver.addTask(self._dingoBaseTask)
            self.qpsolver.addTask(self.postureTask)
        return True

    def reset_callback(self, data):
        self.doorKinematics = mc_solver.KinematicsConstraint(
            self.robots(), 2, self.qpsolver.timeStep
        )
        self.qpsolver.addConstraintSet(self.doorKinematics)
        self.robots().robot(0).posW(
            sva.PTransformd(sva.RotZ(0), eigen.Vector3d(0.0, 0.0, 0.5))
        )
        self.robots().robot(1).posW(
            sva.PTransformd(sva.RotZ(0), eigen.Vector3d(-0.25, 0.0, 0))
        )
        self.robots().robot(2).posW(
            sva.PTransformd(sva.RotZ(math.pi), eigen.Vector3d(2.0, 1.0, 0))
        )
        self.addContact(mc_control.Contact("dingo", "ur5e", "Base", "Base"))

        self._doorPosture = mc_tasks.PostureTask(self.qpsolver, 2, 1.0, 1.0)
        self.qpsolver.addTask(self._doorPosture)
        self._handTask = mc_tasks.SurfaceTransformTask(
            "Tool", self.robots(), 0, 5.0, 1000.0
        )
        self._dingoBaseTask = mc_tasks.EndEffectorTask(
            "base_link", self.robots(), 1, 2.0, 1000.0
        )
        self.qpsolver.addTask(self._dingoBaseTask)
        self.qpsolver.addTask(self.postureTask)
        self.postureTask.target(
            {
                "shoulder_lift_joint".encode("utf-8"): [-math.pi / 2],
            }
        )
        self._dingoBaseTask.add_ef_pose(
            sva.PTransformd(sva.RotZ(0), eigen.Vector3d(1.5, 0.0, 0.0))
        )

    @staticmethod
    def create(robot, dt):
        dingo = mc_rbdyn.get_robot_module("dingo")
        door = mc_rbdyn.get_robot_module("env/door")
        ground = mc_rbdyn.get_robot_module("env/ground")
        return MobileArmController([robot, dingo, door, ground], dt)

__init__.py

1

from mobile_arm_controller import MobileArmController
Next tutorial: mc_rtc::Configuration general purpose configuration