Newer
Older
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
137
138
139
140
141
142
143
144
145
#include "NJointKneeHipController.h"
#include <SimoxUtility/math/distance/angle_between.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/interface/observers/VariantBase.h>
// #include <armar7/rt/units/njoint_controller/common/controller_types.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron/ControllerConfig.aron.generated.h>
// #include <armar7/rt/units/njoint_controller/knee_hip_controller/aron_conversions.h>
namespace armarx
{
const armarx::NJointControllerRegistration<NJointKneeHipController>
registrationControllerNJointKneeHipController(
"NJointKneeHipController");
NJointKneeHipController::NJointKneeHipController(RobotUnit* robotUnit,
const armarx::NJointControllerConfigPtr& config,
const VirtualRobot::RobotPtr&)
{
ARMARX_RT_LOGF("Creating knee hip controller");
ARMARX_CHECK_EXPRESSION(robotUnit);
// config
ConfigPtrT cfg = ConfigPtrT::dynamicCast(config);
ARMARX_CHECK_EXPRESSION(cfg);
// const arondto::Config configData = arondto::Config::FromAron(cfg->config);
// robot
ARMARX_RT_LOGF("Setting up nodes");
{
_rtRobot = useSynchronizedRtRobot();
_rtRobot->setThreadsafe(false);
ARMARX_CHECK_NOT_NULL(_rtRobot);
// _rtKneeNode = _rtRobot->getRobotNode(configData.params.kneeJointName);
// _rtHipNode = _rtRobot->getRobotNode(configData.params.hipJointName);
// _rtHipBodyNode = _rtRobot->getRobotNode(configData.params.hipBodyName);
const auto *kneeJointName = "Knee";
const auto *hipJointName = "Hip";
const auto *hipBodyName = "Hip_body";
_rtKneeNode = _rtRobot->getRobotNode(kneeJointName);
_rtHipNode = _rtRobot->getRobotNode(hipJointName);
_rtHipBodyNode = _rtRobot->getRobotNode(hipBodyName);
ARMARX_CHECK_NOT_NULL(_rtKneeNode);
ARMARX_CHECK_NOT_NULL(_rtHipNode);
ARMARX_CHECK_NOT_NULL(_rtHipBodyNode);
_rtKneeCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
kneeJointName, armarx::ControlModes::Position1DoF);
ARMARX_CHECK_NOT_NULL(_rtKneeCtrlTarget);
_rtHipCtrlTarget = useControlTarget<armarx::ControlTarget1DoFActuatorPosition>(
hipJointName, armarx::ControlModes::Position1DoF);
ARMARX_CHECK_NOT_NULL(_rtHipCtrlTarget);
}
ARMARX_RT_LOGF("Nodes set up successfully");
}
NJointKneeHipController::~NJointKneeHipController() = default;
// void
// KneeHipController::updateConfig(const ::armarx::aron::data::dto::DictPtr& dto,
// const Ice::Current& /*iceCurrent*/)
// {
// // const auto updateConfigDto = arondto::Config::FromAron(dto);
// // Config config;
// // fromAron(updateConfigDto, config);
// // setControlStruct(config);
// }
void
NJointKneeHipController::onInitNJointController()
{
}
void
NJointKneeHipController::onConnectNJointController()
{
}
std::string
NJointKneeHipController::getClassName(const Ice::Current&) const
{
return "KneeHipController";
}
void
NJointKneeHipController::rtRun(const IceUtil::Time& /*sensorValuesTimestamp*/,
const IceUtil::Time& /*timeSinceLastIteration*/)
{
const float currentHipJointValue = _rtHipNode->getJointValue();
const auto hipRotation =
_rtHipBodyNode->getOrientationInRootFrame() * Eigen::Vector3f::UnitY();
// angle between robot root and hip body in the yz-plane
// leaning forwards < 90° = upright < leaning backwards
const float hipAngle = simox::math::angle_between_vec3f_vec3f(
Eigen::Vector3f::UnitY(), hipRotation, Eigen::Vector3f::UnitX());
// hip joint has positive angles leaning forwards (inverse to hipAngle)
float targetHipAngle =
currentHipJointValue + hipAngle - M_PI_2 + rtGetControlStruct().bowAngle;
float targetKneeAngle = rtGetControlStruct().kneeAngle;
// check reachability
const bool targetReachable = _rtKneeNode->checkJointLimits(targetKneeAngle) &&
_rtHipNode->checkJointLimits(targetHipAngle);
if (not targetReachable)
{
// limit joint ranges to avoid damage
targetKneeAngle = std::clamp(
targetKneeAngle, _rtKneeNode->getJointLimitLow(), _rtKneeNode->getJointLimitHigh());
targetHipAngle = std::clamp(
targetHipAngle, _rtHipNode->getJointLimitLow(), _rtHipNode->getJointLimitHigh());
}
_rtKneeCtrlTarget->position = targetKneeAngle;
_rtHipCtrlTarget->position = targetHipAngle;
}
void
NJointKneeHipController::rtPreActivateController()
{
}
void
NJointKneeHipController::rtPostDeactivateController()
{
}
} // namespace armarx