Dual arm controller (sources)

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

DualArmController.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
31
32
33
34
35
36
37
38
39
40
41
42
43

#pragma once

#include <mc_control/mc_controller.h>
#include <mc_solver/CollisionsConstraint.h>
#include <mc_tasks/EndEffectorTask.h>
#include <mc_tasks/PostureTask.h>

#include "api.h"

enum ControllerPhase
{
  IDLE = 0,
  STARTED,
  MOVE
};
enum ControllerState
{
  GO = 0,
  RETURN
};

struct DualArmController_DLLAPI DualArmController : public mc_control::MCController
{
public:
  DualArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config);

  bool run() override;

  void reset(const mc_control::ControllerResetData & reset_data) override;

private:
  void runUr();
  void runKinova();
  ControllerPhase phase_ = IDLE;
  ControllerState urState_ = RETURN;
  ControllerState kinovaState_ = RETURN;
  std::shared_ptr<mc_tasks::PostureTask> kinovaPostureTask_;
  std::shared_ptr<mc_tasks::EndEffectorTask> urEndEffectorTask_;
  std::unique_ptr<mc_solver::KinematicsConstraint> kinovaKinematics_;

  const double iDist = 0.1;
  const double sDist = 0.05;
};

DualArmController.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 "DualArmController.h"

#include <mc_rbdyn/RobotLoader.h>

DualArmController::DualArmController(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config)
: mc_control::MCController({rm, mc_rbdyn::RobotLoader::get_robot_module("KinovaDefault")}, dt)
{
  solver().addConstraintSet(contactConstraint);
  solver().addConstraintSet(kinematicsConstraint);
  solver().addConstraintSet(selfCollisionConstraint);

  addCollisions("ur5e", "kinova_default", {{"*", "*", iDist, sDist, 0}});

  postureTask->stiffness(1);
  postureTask->weight(1);
  solver().addTask(postureTask);

  solver().setContacts({{}});
}

bool DualArmController::run()
{
  if(phase_ == IDLE)
  {
    postureTask->target({{"elbow_joint", {-M_PI / 2}}, {"wrist_2_joint", {M_PI / 2}}});
    phase_ = STARTED;
  }
  else if(phase_ == STARTED && postureTask->eval().norm() < 0.01 && postureTask->speed().norm() < 0.01)
  {
    phase_ = MOVE;
    solver().addTask(urEndEffectorTask_);
  }
  else if(phase_ == STARTED) { urEndEffectorTask_->reset(); }
  else if(phase_ == MOVE)
  {
    runUr();
    runKinova();
  }
  return mc_control::MCController::run();
}

void DualArmController::runUr()
{
  if(urState_ == GO && urEndEffectorTask_->eval().norm() < 0.05 && urEndEffectorTask_->speed().norm() < 0.01)
  {
    urState_ = RETURN;
    urEndEffectorTask_->add_ef_pose({Eigen::Vector3d(0.0, -0.5, 0.0)});
  }
  else if(urState_ == RETURN && urEndEffectorTask_->eval().norm() < 0.01 && urEndEffectorTask_->speed().norm() < 0.05)
  {
    urState_ = GO;
    urEndEffectorTask_->add_ef_pose({Eigen::Vector3d(0.0, 0.5, 0.0)});
  }
}

void DualArmController::runKinova()
{
  if(kinovaState_ == GO && kinovaPostureTask_->eval().norm() < 0.01 && kinovaPostureTask_->speed().norm() < 0.01)
  {
    kinovaState_ = RETURN;
    kinovaPostureTask_->target({{"joint_2", {0.0}}});
  }
  else if(kinovaState_ == RETURN && kinovaPostureTask_->eval().norm() < 0.01
          && kinovaPostureTask_->speed().norm() < 0.01)
  {
    kinovaState_ = GO;
    kinovaPostureTask_->target({{"joint_2", {-M_PI / 4}}});
  }
}

void DualArmController::reset(const mc_control::ControllerResetData & reset_data)
{
  mc_control::MCController::reset(reset_data);

  std::string urBody = "tool0";
  urEndEffectorTask_ = std::make_shared<mc_tasks::EndEffectorTask>(urBody, robots(), 0, 1);

  kinovaPostureTask_ = std::make_shared<mc_tasks::PostureTask>(solver(), 1, 1, 1);
  solver().addTask(kinovaPostureTask_);

  kinovaKinematics_ = std::make_unique<mc_solver::KinematicsConstraint>(robots(), 1, solver().dt());
  solver().addConstraintSet(kinovaKinematics_);

  robots().robot(1).posW(sva::PTransformd(sva::RotZ(0.0), Eigen::Vector3d(0.7, 0.5, 0)));
}

CONTROLLER_CONSTRUCTOR("DualArmController", DualArmController)

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

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


class ControllerPhase:
    IDLE = 0
    STARTED = 1
    MOVE = 2


class ControllerState:
    GO = 0
    RETURN = 1


class DualArmController(mc_control.MCPythonController):
    def __init__(self, rm, dt):
        self.qpsolver.addConstraintSet(self.contactConstraint)
        self.qpsolver.addConstraintSet(self.kinematicsConstraint)
        self.qpsolver.addConstraintSet(self.selfCollisionConstraint)
        iDist, sDist, damping = 0.1, 0.05, 0.1
        self.addCollisions(
            "ur5e",
            "kinova_default",
            [mc_rbdyn.Collision("*", "*", iDist, sDist, damping)],
        )
        self.postureTask.stiffness(1)
        self.postureTask.weight(1)
        self.qpsolver.addTask(self.postureTask)
        self._phase = ControllerPhase.IDLE
        self._ur_state = ControllerState.RETURN
        self._kinova_state = ControllerState.RETURN
        self._urEndEffectorTask = None
        self._kinovaPostureTask = None
        self._kinovaKinematicsConstraint = None

    def run_callback(self):
        if self._phase == ControllerPhase.IDLE:
            self.postureTask.target(
                {
                    "elbow_joint".encode("utf-8"): [-math.pi / 2],
                    "wrist_2_joint".encode("utf-8"): [math.pi / 2],
                }
            )
            self._phase = ControllerPhase.STARTED
        elif (
            self._phase == ControllerPhase.STARTED
            and self.postureTask.eval().norm() < 0.01
            and self.postureTask.speed().norm() < 0.01
        ):
            self._phase = ControllerPhase.MOVE
            self.qpsolver.addTask(self._urEndEffectorTask)
        elif self._phase == ControllerPhase.STARTED:
            self._urEndEffectorTask.reset()
        elif self._phase == ControllerPhase.MOVE:
            self._run_ur5e()
            self._run_kinova()
        return True

    def reset_callback(self, data):
        self._urEndEffectorTask = mc_tasks.EndEffectorTask("tool0", self.robots(), 0, 1)

        self._kinovaPostureTask = mc_tasks.PostureTask(self.qpsolver, 1, 5.0, 1000.0)
        self._kinovaKinematicsConstraint = mc_solver.KinematicsConstraint(
            self.robots(), 1, self.qpsolver.timeStep
        )
        self.qpsolver.addTask(self._kinovaPostureTask)
        self.qpsolver.addConstraintSet(self._kinovaKinematicsConstraint)
        self.robots().robot(1).posW(
            sva.PTransformd(sva.RotZ(0), eigen.Vector3d(0.7, 0.5, 0))
        )

    @staticmethod
    def create(robot, dt):
        kinova = mc_rbdyn.get_robot_module("KinovaDefault")
        return DualArmController([robot, kinova], dt)

    def _run_kinova(self):
        if (
            self._kinova_state == ControllerState.GO
            and self._kinovaPostureTask.eval().norm() < 0.01
            and self._kinovaPostureTask.speed().norm() < 0.01
        ):
            self._kinova_state = ControllerState.RETURN
            self._kinovaPostureTask.target(
                {
                    "joint_2".encode("utf-8"): [0.0],
                }
            )
        elif (
            self._kinova_state == ControllerState.RETURN
            and self._kinovaPostureTask.eval().norm() < 0.01
            and self._kinovaPostureTask.speed().norm() < 0.01
        ):
            self._kinova_state = ControllerState.GO
            self._kinovaPostureTask.target(
                {
                    "joint_2".encode("utf-8"): [-math.pi / 4],
                }
            )

    def _run_ur5e(self):
        if (
            self._ur_state == ControllerState.GO
            and self._urEndEffectorTask.positionTask.eval().norm() < 0.05
            and self._urEndEffectorTask.positionTask.speed().norm() < 0.05
        ):
            self._ur_state = ControllerState.RETURN
            self._urEndEffectorTask.add_ef_pose(
                sva.PTransformd(sva.RotZ(0), eigen.Vector3d(0.0, -0.5, 0.0))
            )
        elif (
            self._ur_state == ControllerState.RETURN
            and self._urEndEffectorTask.positionTask.eval().norm() < 0.05
            and self._urEndEffectorTask.positionTask.speed().norm() < 0.05
        ):
            self._ur_state = ControllerState.GO
            self._urEndEffectorTask.add_ef_pose(
                sva.PTransformd(sva.RotZ(0), eigen.Vector3d(0.0, 0.5, 0.0))
            )

__init__.py

1

from my_first_controller import MyFirstController
Next tutorial: Mobile arm controller