Skip to content

Commit 1c56cb1

Browse files
costashatzcostashatz
authored
Merge pull request #142 from resibots/pd_ctrl_angular
Rotational error added to pd controller
2 parents d464b0b + 93cf715 commit 1c56cb1

File tree

3 files changed

+105
-12
lines changed

3 files changed

+105
-12
lines changed

‎src/python/control.cpp

Lines changed: 10 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -103,12 +103,14 @@ namespace robot_dart {
103103
// PDControl class
104104
py::class_<PDControl, RobotControl, std::shared_ptr<PDControl>>(m, "PDControl")
105105
.def(py::init<>())
106-
.def(py::init<const Eigen::VectorXd&, bool>(),
106+
.def(py::init<const Eigen::VectorXd&, bool, bool>(),
107107
py::arg("ctrl"),
108-
py::arg("full_control") = false)
109-
.def(py::init<const Eigen::VectorXd&, const std::vector<std::string>&>(),
108+
py::arg("full_control") = false,
109+
py::arg("use_angular_errors") = true)
110+
.def(py::init<const Eigen::VectorXd&, const std::vector<std::string>&, bool>(),
110111
py::arg("ctrl"),
111-
py::arg("controllable_dofs"))
112+
py::arg("controllable_dofs"),
113+
py::arg("use_angular_errors") = true)
112114

113115
.def("configure", &PDControl::configure)
114116
.def("calculate", &PDControl::calculate)
@@ -118,6 +120,10 @@ namespace robot_dart {
118120

119121
.def("pd", &PDControl::pd)
120122

123+
.def("using_angular_errors", &PDControl::using_angular_errors)
124+
.def("set_use_angular_errors", &PDControl::set_use_angular_errors,
125+
py::arg("enable") = true)
126+
121127
.def("clone", &PDControl::clone);
122128

123129
// SimpleControl class

‎src/robot_dart/control/pd_control.cpp

Lines changed: 87 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,11 +2,16 @@
22
#include "robot_dart/robot.hpp"
33
#include "robot_dart/utils.hpp"
44

5+
#include <dart/dynamics/BallJoint.hpp>
6+
#include <dart/dynamics/EulerJoint.hpp>
7+
#include <dart/dynamics/FreeJoint.hpp>
8+
#include <dart/dynamics/RevoluteJoint.hpp>
9+
510
namespace robot_dart {
611
namespace control {
712
PDControl::PDControl() : RobotControl() {}
8-
PDControl::PDControl(const Eigen::VectorXd& ctrl, bool full_control) : RobotControl(ctrl, full_control) {}
9-
PDControl::PDControl(const Eigen::VectorXd& ctrl, const std::vector<std::string>& controllable_dofs) : RobotControl(ctrl, controllable_dofs) {}
13+
PDControl::PDControl(const Eigen::VectorXd& ctrl, bool full_control, bool use_angular_errors) : RobotControl(ctrl, full_control), _use_angular_errors(use_angular_errors) {}
14+
PDControl::PDControl(const Eigen::VectorXd& ctrl, const std::vector<std::string>& controllable_dofs, bool use_angular_errors) : RobotControl(ctrl, controllable_dofs), _use_angular_errors(use_angular_errors) {}
1015

1116
void PDControl::configure()
1217
{
@@ -21,14 +26,77 @@ namespace robot_dart {
2126
{
2227
ROBOT_DART_ASSERT(_control_dof == _ctrl.size(), "PDControl: Controller parameters size is not the same as DOFs of the robot", Eigen::VectorXd::Zero(_control_dof));
2328
auto robot = _robot.lock();
24-
Eigen::VectorXd& target_positions = _ctrl;
2529

26-
Eigen::VectorXd q = robot->positions(_controllable_dofs);
2730
Eigen::VectorXd dq = robot->velocities(_controllable_dofs);
2831

32+
Eigen::VectorXd error;
33+
if (!_use_angular_errors) {
34+
Eigen::VectorXd q = robot->positions(_controllable_dofs);
35+
error = _ctrl - q;
36+
}
37+
else {
38+
error = Eigen::VectorXd::Zero(_control_dof);
39+
40+
std::unordered_map<size_t, Eigen::VectorXd> joint_vals, joint_desired, errors;
41+
42+
for (int i = 0; i < _control_dof; ++i) {
43+
auto dof = robot->dof(_controllable_dofs[i]);
44+
size_t joint_index = dof->getJoint()->getJointIndexInSkeleton();
45+
if (joint_vals.find(joint_index) == joint_vals.end()) {
46+
joint_vals[joint_index] = dof->getJoint()->getPositions();
47+
joint_desired[joint_index] = dof->getJoint()->getPositions();
48+
}
49+
50+
joint_desired[joint_index][dof->getIndexInJoint()] = _ctrl[i];
51+
}
52+
53+
for (int i = 0; i < _control_dof; ++i) {
54+
auto dof = robot->dof(_controllable_dofs[i]);
55+
size_t joint_index = dof->getJoint()->getJointIndexInSkeleton();
56+
size_t dof_index_in_joint = dof->getIndexInJoint();
57+
58+
Eigen::VectorXd val;
59+
if (errors.find(joint_index) == errors.end()) {
60+
val = Eigen::VectorXd(dof->getJoint()->getNumDofs());
61+
62+
std::string joint_type = robot->dof(_controllable_dofs[i])->getJoint()->getType();
63+
if (joint_type == dart::dynamics::RevoluteJoint::getStaticType()) {
64+
val[dof_index_in_joint] = _angle_dist(_ctrl[i], joint_vals[joint_index][dof_index_in_joint]);
65+
}
66+
else if (joint_type == dart::dynamics::BallJoint::getStaticType()) {
67+
Eigen::Matrix3d R_desired = dart::math::expMapRot(joint_desired[joint_index]);
68+
Eigen::Matrix3d R_current = dart::math::expMapRot(joint_vals[joint_index]);
69+
val = dart::math::logMap(R_desired * R_current.transpose());
70+
}
71+
else if (joint_type == dart::dynamics::EulerJoint::getStaticType()) {
72+
// TO-DO: Check if this is 100% correct
73+
for (size_t d = 0; d < dof->getJoint()->getNumDofs(); d++)
74+
val[d] = _angle_dist(joint_desired[joint_index][d], joint_vals[joint_index][d]);
75+
}
76+
else if (joint_type == dart::dynamics::FreeJoint::getStaticType()) {
77+
auto free_joint = static_cast<dart::dynamics::FreeJoint*>(dof->getJoint());
78+
79+
Eigen::Isometry3d tf_desired = free_joint->convertToTransform(joint_desired[joint_index]);
80+
Eigen::Isometry3d tf_current = free_joint->convertToTransform(joint_vals[joint_index]);
81+
82+
val.tail(3) = tf_desired.translation() - tf_current.translation();
83+
val.head(3) = dart::math::logMap(tf_desired.linear().matrix() * tf_current.linear().matrix().transpose());
84+
}
85+
else {
86+
val[dof_index_in_joint] = _ctrl[i] - joint_vals[joint_index][dof_index_in_joint];
87+
}
88+
89+
errors[joint_index] = val;
90+
}
91+
else
92+
val = errors[joint_index];
93+
error(i) = val[dof_index_in_joint];
94+
}
95+
}
96+
2997
/// Compute the simplest PD controller output:
3098
/// P gain * (target position - current position) + D gain * (0 - current velocity)
31-
Eigen::VectorXd commands = _Kp.array() * (target_positions.array() - q.array()) - _Kd.array() * dq.array();
99+
Eigen::VectorXd commands = _Kp.array() * error.array() - _Kd.array() * dq.array();
32100

33101
return commands;
34102
}
@@ -52,9 +120,23 @@ namespace robot_dart {
52120
return std::make_pair(_Kp, _Kd);
53121
}
54122

123+
bool PDControl::using_angular_errors() const { return _use_angular_errors; }
124+
125+
void PDControl::set_use_angular_errors(bool enable) { _use_angular_errors = enable; }
126+
55127
std::shared_ptr<RobotControl> PDControl::clone() const
56128
{
57129
return std::make_shared<PDControl>(*this);
58130
}
131+
132+
double PDControl::_angle_dist(double target, double current)
133+
{
134+
double theta = target - current;
135+
while (theta < -M_PI)
136+
theta += 2 * M_PI;
137+
while (theta > M_PI)
138+
theta -= 2 * M_PI;
139+
return theta;
140+
}
59141
} // namespace control
60142
} // namespace robot_dart

‎src/robot_dart/control/pd_control.hpp

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@ namespace robot_dart {
1313
class PDControl : public RobotControl {
1414
public:
1515
PDControl();
16-
PDControl(const Eigen::VectorXd& ctrl, bool full_control = false);
17-
PDControl(const Eigen::VectorXd& ctrl, const std::vector<std::string>& controllable_dofs);
16+
PDControl(const Eigen::VectorXd& ctrl, bool full_control = false, bool use_angular_errors = true);
17+
PDControl(const Eigen::VectorXd& ctrl, const std::vector<std::string>& controllable_dofs, bool use_angular_errors = true);
1818

1919
void configure() override;
2020
Eigen::VectorXd calculate(double) override;
@@ -24,13 +24,18 @@ namespace robot_dart {
2424

2525
std::pair<Eigen::VectorXd, Eigen::VectorXd> pd() const;
2626

27+
bool using_angular_errors() const;
28+
void set_use_angular_errors(bool enable = true);
29+
2730
std::shared_ptr<RobotControl> clone() const override;
2831

2932
protected:
3033
Eigen::VectorXd _Kp;
3134
Eigen::VectorXd _Kd;
35+
bool _use_angular_errors;
36+
37+
static double _angle_dist(double target, double current);
3238
};
3339
} // namespace control
3440
} // namespace robot_dart
35-
3641
#endif

0 commit comments

Comments
 (0)