From fcc6b861aa28f7d13bbc367d99d4e9257c270e7d Mon Sep 17 00:00:00 2001
From: Christoph Pohl <christoph.pohl@kit.edu>
Date: Mon, 22 Nov 2021 12:47:11 +0100
Subject: [PATCH] Added new interface for querying FrameTracking
---
.../FrameTracking/FrameTracking.cpp | 472 ++++++++++--------
.../components/FrameTracking/FrameTracking.h | 2 +
.../components/FrameTrackingInterface.ice | 1 +
3 files changed, 277 insertions(+), 198 deletions(-)
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
index 89f5f9341..f8ad1a043 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.cpp
@@ -58,9 +58,12 @@ namespace armarx
void FrameTracking::onConnectComponent()
{
- robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(getProperty<std::string>("RobotStateComponentName").getValue());
- kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
- kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(getProperty<std::string>("KinematicUnitObserverName").getValue());
+ robotStateComponent = getProxy<RobotStateComponentInterfacePrx>(
+ getProperty<std::string>("RobotStateComponentName").getValue());
+ kinematicUnitInterfacePrx = getProxy<KinematicUnitInterfacePrx>(
+ getProperty<std::string>("KinematicUnitName").getValue());
+ kinematicUnitObserverInterfacePrx = getProxy<KinematicUnitObserverInterfacePrx>(
+ getProperty<std::string>("KinematicUnitObserverName").getValue());
localRobot = armarx::RemoteRobot::createLocalClone(robotStateComponent);
headYawJoint = localRobot->getRobotNode(getProperty<std::string>("HeadYawJoint").getValue());
@@ -91,151 +94,167 @@ namespace armarx
RemoteGui::detail::VBoxLayoutBuilder rootLayoutBuilder = RemoteGui::makeVBoxLayout();
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Tracking: "))
- .addChild(RemoteGui::makeTextLabel("Enabled"))
- .addChild(RemoteGui::makeCheckBox("enabled").value(enabled))
- .addChild(RemoteGui::makeTextLabel("Frame"))
- .addChild(
- RemoteGui::makeComboBox("tracking_frame")
- .options(localRobot->getRobotNodeNames())
- .addOptions({""})
- .value(frameName)
- )
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Tracking: ")).addChild(
+ RemoteGui::makeTextLabel("Enabled")).addChild(
+ RemoteGui::makeCheckBox("enabled").value(enabled)).addChild(
+ RemoteGui::makeTextLabel("Frame")).addChild(
+ RemoteGui::makeComboBox("tracking_frame").options(
+ localRobot->getRobotNodeNames()).addOptions({""}).value(frameName)));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at frame: "))
- .addChild(
- RemoteGui::makeComboBox("frame_look")
- .options(localRobot->getRobotNodeNames())
- .value(frameName)
- )
- .addChild(RemoteGui::makeButton("button_look_at_frame").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at frame: ")).addChild(
+ RemoteGui::makeComboBox("frame_look").options(localRobot->getRobotNodeNames()).value(
+ frameName)).addChild(
+ RemoteGui::makeButton("button_look_at_frame").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at global point: "))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_global_point").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Look at global point: ")).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_x").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_y").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("global_point_z").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeButton("button_look_at_global_point").label("look at")));
+
+ rootLayoutBuilder.addChild(RemoteGui::makeHBoxLayout().addChild(
+ RemoteGui::makeTextLabel("Look at point in robot frame: ")).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(
+ 2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeButton("button_look_at_robot_point").label("look at")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Look at point in robot frame: "))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("robot_point_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeButton("button_look_at_robot_point").label("look at"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
+ RemoteGui::makeTextLabel("yaw from ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(
+ headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("yaw to ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(
+ headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("pitch ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(
+ headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(
+ static_cast<int>(
+ (headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) /
+ 0.1))).addChild(RemoteGui::makeTextLabel("velocity ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(
+ static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
+ RemoteGui::makeButton("button_scan_in_configuration_space").label("scan")));
rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("yaw from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_from").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("yaw to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_yaw_to").min(headYawJoint->getJointLimitLo()).max(headYawJoint->getJointLimitHi()).steps(static_cast<int>((headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("pitch "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_pitch").min(headPitchJoint->getJointLimitLo()).max(headPitchJoint->getJointLimitHi()).steps(static_cast<int>((headPitchJoint->getJointLimitHi() - headPitchJoint->getJointLimitLo()) / 0.1)))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_configuration_space_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeButton("button_scan_in_configuration_space").label("scan"))
- );
-
- rootLayoutBuilder.addChild(
- RemoteGui::makeHBoxLayout()
- .addChild(RemoteGui::makeTextLabel("Scan: "))
- .addChild(RemoteGui::makeTextLabel("from "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("to "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(1000000000).steps(2 * 1000000000 / 10).value(0.f))
- .addChild(RemoteGui::makeTextLabel("velocity "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(static_cast<int>(6 / 0.1)).value(0.8f))
- .addChild(RemoteGui::makeTextLabel("acceleration "))
- .addChild(RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(static_cast<int>(8 / 0.1)).value(4.0f))
- .addChild(RemoteGui::makeButton("button_scan_in_workspace").label("scan"))
- );
+ RemoteGui::makeHBoxLayout().addChild(RemoteGui::makeTextLabel("Scan: ")).addChild(
+ RemoteGui::makeTextLabel("from ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_x").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_y").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_from_z").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeTextLabel("to ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_x").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_y").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_to_z").min(-1000000000).max(
+ 1000000000).steps(2 * 1000000000 / 10).value(0.f)).addChild(
+ RemoteGui::makeTextLabel("velocity ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_velocity").min(0).max(6).steps(
+ static_cast<int>(6 / 0.1)).value(0.8f)).addChild(
+ RemoteGui::makeTextLabel("acceleration ")).addChild(
+ RemoteGui::makeFloatSpinBox("scan_in_workspace_acceleration").min(0).max(8).steps(
+ static_cast<int>(8 / 0.1)).value(4.0f)).addChild(
+ RemoteGui::makeButton("button_scan_in_workspace").label("scan")));
rootLayoutBuilder.addChild(new RemoteGui::VSpacer());
_guiTask = new SimplePeriodicTask<>([this]()
- {
- bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
- std::string oldFrameGui = _guiTab.getValue<std::string>("tracking_frame").get();
-
- _guiTab.receiveUpdates();
-
- if (oldEnabledGui == enabled)
- {
- // only apply changes of gui if not already changed by ice
- _enableTracking(_guiTab.getValue<bool>("enabled").get());
- }
- _guiTab.getValue<bool>("enabled").set(enabled);
-
- if (oldFrameGui == frameName && oldFrameGui != _guiTab.getValue<std::string>("tracking_frame").get())
- {
- // only apply changes of gui if not already changed by ice
- setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
- }
- _guiTab.getValue<std::string>("tracking_frame").set(frameName);
-
- _guiTab.sendUpdates();
-
- if (_guiTab.getButton("button_look_at_frame").clicked())
- {
- lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
- }
-
- if (_guiTab.getButton("button_look_at_global_point").clicked())
- {
- lookAtPointInGlobalFrame(Vector3f{_guiTab.getValue<float>("global_point_x").get(),
- _guiTab.getValue<float>("global_point_y").get(),
- _guiTab.getValue<float>("global_point_z").get()});
- }
-
- if (_guiTab.getButton("button_look_at_robot_point").clicked())
- {
- lookAtPointInRobotFrame(Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
- _guiTab.getValue<float>("robot_point_y").get(),
- _guiTab.getValue<float>("robot_point_z").get()});
- }
-
- if (_guiTab.getButton("button_scan_in_configuration_space").clicked())
- {
- scanInConfigurationSpace(_guiTab.getValue<float>("scan_in_configuration_space_yaw_from").get(),
- _guiTab.getValue<float>("scan_in_configuration_space_yaw_to").get(),
- {_guiTab.getValue<float>("scan_in_configuration_space_pitch").get()},
- _guiTab.getValue<float>("scan_in_configuration_space_velocity").get());
- }
-
- if (_guiTab.getButton("button_scan_in_workspace").clicked())
- {
- scanInWorkspace(
- {
- {
- _guiTab.getValue<float>("scan_in_workspace_from_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_from_z").get()
- },
- {
- _guiTab.getValue<float>("scan_in_workspace_to_x").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_y").get(),
- _guiTab.getValue<float>("scan_in_workspace_to_z").get()
- }
- },
- _guiTab.getValue<float>("scan_in_workspace_velocity").get(),
- _guiTab.getValue<float>("scan_in_workspace_acceleration").get());
- }
- }, 10);
+ {
+ bool oldEnabledGui = _guiTab.getValue<bool>("enabled").get();
+ std::string oldFrameGui = _guiTab.getValue<std::string>(
+ "tracking_frame").get();
+
+ _guiTab.receiveUpdates();
+
+ if (oldEnabledGui == enabled)
+ {
+ // only apply changes of gui if not already changed by ice
+ _enableTracking(_guiTab.getValue<bool>("enabled").get());
+ }
+ _guiTab.getValue<bool>("enabled").set(enabled);
+
+ if (oldFrameGui == frameName && oldFrameGui !=
+ _guiTab.getValue<std::string>(
+ "tracking_frame").get())
+ {
+ // only apply changes of gui if not already changed by ice
+ setFrame(_guiTab.getValue<std::string>("tracking_frame").get());
+ }
+ _guiTab.getValue<std::string>("tracking_frame").set(frameName);
+
+ _guiTab.sendUpdates();
+
+ if (_guiTab.getButton("button_look_at_frame").clicked())
+ {
+ lookAtFrame(_guiTab.getValue<std::string>("frame_look").get());
+ }
+
+ if (_guiTab.getButton("button_look_at_global_point").clicked())
+ {
+ lookAtPointInGlobalFrame(Vector3f{
+ _guiTab.getValue<float>("global_point_x").get(),
+ _guiTab.getValue<float>("global_point_y").get(),
+ _guiTab.getValue<float>("global_point_z").get()});
+ }
+
+ if (_guiTab.getButton("button_look_at_robot_point").clicked())
+ {
+ lookAtPointInRobotFrame(
+ Vector3f{_guiTab.getValue<float>("robot_point_x").get(),
+ _guiTab.getValue<float>("robot_point_y").get(),
+ _guiTab.getValue<float>(
+ "robot_point_z").get()});
+ }
+
+ if (_guiTab.getButton(
+ "button_scan_in_configuration_space").clicked())
+ {
+ scanInConfigurationSpace(_guiTab.getValue<float>(
+ "scan_in_configuration_space_yaw_from").get(),
+ _guiTab.getValue<float>(
+ "scan_in_configuration_space_yaw_to").get(),
+ {_guiTab.getValue<float>(
+ "scan_in_configuration_space_pitch").get()},
+ _guiTab.getValue<float>(
+ "scan_in_configuration_space_velocity").get());
+ }
+
+ if (_guiTab.getButton("button_scan_in_workspace").clicked())
+ {
+ scanInWorkspace({{_guiTab.getValue<float>(
+ "scan_in_workspace_from_x").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_from_y").get(), _guiTab.getValue<
+ float>("scan_in_workspace_from_z").get()},
+ {_guiTab.getValue<float>(
+ "scan_in_workspace_to_x").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_to_y").get(), _guiTab.getValue<
+ float>(
+ "scan_in_workspace_to_z").get()}},
+ _guiTab.getValue<float>(
+ "scan_in_workspace_velocity").get(),
+ _guiTab.getValue<float>(
+ "scan_in_workspace_acceleration").get());
+ }
+ }, 10);
RemoteGui::WidgetPtr rootLayout = rootLayoutBuilder;
@@ -265,8 +284,7 @@ namespace armarx
armarx::PropertyDefinitionsPtr FrameTracking::createPropertyDefinitions()
{
- return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(
- getConfigIdentifier()));
+ return armarx::PropertyDefinitionsPtr(new FrameTrackingPropertyDefinitions(getConfigIdentifier()));
}
void FrameTracking::enableTracking(bool enable, const Ice::Current&)
@@ -316,6 +334,17 @@ namespace armarx
_lookAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)));
}
+ bool FrameTracking::isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current&)
+ {
+ if (enabled)
+ {
+ ARMARX_WARNING << "Disable tracking to use lookAt functions.";
+ return false;
+ }
+ syncronizeLocalClone();
+ return _looksAtPoint(localRobot->toLocalCoordinateSystemVec(ToEigen(point)), max_diff);
+ }
+
void FrameTracking::lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current&)
{
if (enabled)
@@ -332,8 +361,12 @@ namespace armarx
const float currentYaw = headYawJoint->getJointValue();
const float currentPitch = headPitchJoint->getJointValue();
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headPitchJoint->getName()))->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -346,21 +379,22 @@ namespace armarx
headState.desiredPitchPos = pitch;
_doPositionControl(headState);
struct timespec req = {0, 30 * 1000000L};
- while (
- std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
- std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.)
- )
+ while (std::abs(headYawJoint->getJointValue() - yaw) > static_cast<float>(M_PI / 180.) ||
+ std::abs(headPitchJoint->getJointValue() - pitch) > static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: " << headPitchJoint->getJointValue() << " -> " << pitch;
+ ARMARX_INFO << "yaw: " << headYawJoint->getJointValue() << " -> " << yaw << " pitch: "
+ << headPitchJoint->getJointValue() << " -> " << pitch;
syncronizeLocalClone();
// sleep for 30 milliseconds
nanosleep(&req, nullptr);
}
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues, float velocity, const Ice::Current&)
+ void FrameTracking::scanInConfigurationSpace(float yawFrom, float yawTo, const Ice::FloatSeq& pitchValues,
+ float velocity, const Ice::Current&)
{
if (enabled)
{
@@ -371,28 +405,33 @@ namespace armarx
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
// to initial yaw
{
bool wasGreater = headYawJoint->getJointValue() > yawFrom;
float yawVelocityToInit = wasGreater ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), yawVelocityToInit},
+ {headPitchJoint->getName(), 0.f}});
// if the joint angle was greater before we want to run as long as it is greater
// otherwise we want to run as long as it is smaler
- while ((wasGreater && headYawJoint->getJointValue() > yawFrom) || (!wasGreater && headYawJoint->getJointValue() < yawFrom))
+ while ((wasGreater && headYawJoint->getJointValue() > yawFrom) ||
+ (!wasGreater && headYawJoint->getJointValue() < yawFrom))
{
syncronizeLocalClone();
}
}
- for (const auto& p : pitchValues)
+ for (const auto& p: pitchValues)
{
// to pitch value
bool wasGreaterP = headPitchJoint->getJointValue() > p;
float velocityPitch = wasGreaterP ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), velocityPitch}});
- while ((wasGreaterP && headPitchJoint->getJointValue() > p) || (!wasGreaterP && headPitchJoint->getJointValue() < p))
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), velocityPitch}});
+ while ((wasGreaterP && headPitchJoint->getJointValue() > p) ||
+ (!wasGreaterP && headPitchJoint->getJointValue() < p))
{
syncronizeLocalClone();
}
@@ -400,19 +439,24 @@ namespace armarx
// to yaw value
bool wasGreaterY = yawFrom > yawTo; // yawFrom == headYawJoint->getJointValue()
float velocityYaw = wasGreaterY ? -velocity : velocity;
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw}, {headPitchJoint->getName(), 0.f}});
- while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) || (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), velocityYaw},
+ {headPitchJoint->getName(), 0.f}});
+ while ((wasGreaterY && headYawJoint->getJointValue() > yawTo) ||
+ (!wasGreaterY && headYawJoint->getJointValue() < yawTo))
{
syncronizeLocalClone();
}
std::swap(yawFrom, yawTo);
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
- void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration, const Ice::Current&)
+ void FrameTracking::scanInWorkspace(const Vector3fSeq& points, float maxVelocity, float acceleration,
+ const Ice::Current&)
{
if (enabled)
{
@@ -421,16 +465,18 @@ namespace armarx
}
syncronizeLocalClone();
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
struct timespec req = {0, 30 * 1000000L};
- for (const auto& p : points)
+ for (const auto& p: points)
{
auto pEigen = localRobot->toLocalCoordinateSystemVec(ToEigen(p));
auto target = _calculateJointAngles(pEigen);
- while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
+ while (std::abs(target.currentYawPos - target.desiredYawPos) > static_cast<float>(M_PI / 180.) ||
std::abs(target.currentPitchPos - target.desiredPitchPos) > static_cast<float>(M_PI / 180.))
{
- ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: " << target.currentPitchPos << " - " << target.desiredPitchPos;
+ ARMARX_INFO << "yaw: " << target.currentYawPos << " - " << target.desiredYawPos << " pitch: "
+ << target.currentPitchPos << " - " << target.desiredPitchPos;
syncronizeLocalClone();
target = _calculateJointAngles(pEigen);
_doVelocityControl(target, maxVelocity, acceleration, maxVelocity, acceleration);
@@ -438,8 +484,10 @@ namespace armarx
nanosleep(&req, nullptr);
}
}
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
void FrameTracking::process()
@@ -448,13 +496,14 @@ namespace armarx
{
ARMARX_ERROR << frameName << " does not exist. Task will be disabled.";
std::thread([this]()
- {
- _enableTracking(false);
- }).detach();
+ {
+ _enableTracking(false);
+ }).detach();
return;
}
syncronizeLocalClone();
- _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration, maxPitchVelocity, pitchAcceleration);
+ _doVelocityControl(_calculateJointAnglesContinously(frameName), maxYawVelocity, yawAcceleration,
+ maxPitchVelocity, pitchAcceleration);
}
void FrameTracking::syncronizeLocalClone()
@@ -474,13 +523,21 @@ namespace armarx
_doPositionControl(_calculateJointAngles(point));
}
+ bool FrameTracking::_looksAtPoint(const Eigen::Vector3f& point, float max_diff)
+ {
+ auto head_state = _calculateJointAngles(point);
+ float diff = std::abs(head_state.desiredPitchPos - head_state.currentPitchPos) +
+ std::abs(head_state.currentYawPos - head_state.desiredYawPos);
+ return max_diff > diff;
+ }
+
FrameTracking::HeadState FrameTracking::_calculateJointAnglesContinously(const std::string& frameName)
{
auto frame = localRobot->getRobotNode(frameName);
auto posInRobotFrame = localRobot->toLocalCoordinateSystemVec(frame->getGlobalPosition());
// do nothing if the robot works above his head
// he should already look upwards because if this component runs continously
- if (std::sqrt(posInRobotFrame.x()*posInRobotFrame.x() + posInRobotFrame.y()*posInRobotFrame.y()) < 300.f)
+ if (std::sqrt(posInRobotFrame.x() * posInRobotFrame.x() + posInRobotFrame.y() * posInRobotFrame.y()) < 300.f)
{
return FrameTracking::HeadState{true, 0.f, 0.f, 0.f, 0.f, 0.f, 0.f};
}
@@ -495,24 +552,34 @@ namespace armarx
yaw = std::min(headYawJoint->getJointLimitHi(), yaw);
// we dont want the robot to move from one limit to the other in one step
const float currentYaw = headYawJoint->getJointValue();
- if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) > headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() - static_cast<float>(M_PI) / 8)
+ if (!headYawJoint->isLimitless() && std::abs(currentYaw - yaw) >
+ headYawJoint->getJointLimitHi() - headYawJoint->getJointLimitLo() -
+ static_cast<float>(M_PI) / 8)
{
yaw = currentYaw;
}
- const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(localRobot->toGlobalCoordinateSystemVec(point));
+ const auto pointInPitchJointFrame = headPitchJoint->toLocalCoordinateSystemVec(
+ localRobot->toGlobalCoordinateSystemVec(point));
const Eigen::Vector2f pj{pointInPitchJointFrame.y(), pointInPitchJointFrame.z()};
- const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(cameraNode->getGlobalPosition()).z();
- float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) + std::asin(headHeightRealativeToPitchJoint / pj.norm());
+ const float headHeightRealativeToPitchJoint = headPitchJoint->toLocalCoordinateSystemVec(
+ cameraNode->getGlobalPosition()).z();
+ float pitch = headPitchJoint->getJointValue() - std::asin(pj.x() / pj.norm()) +
+ std::asin(headHeightRealativeToPitchJoint / pj.norm());
// make shure the joint value satisfies the joint limits
pitch = std::max(headPitchJoint->getJointLimitLo(), pitch);
pitch = std::min(headPitchJoint->getJointLimitHi(), pitch);
const float currentPitch = headPitchJoint->getJointValue();
- ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw << " and pitch=" << pitch;
+ ARMARX_INFO << deactivateSpam(1.f, "FrameTracking") << "Looking at " << point << " using yaw=" << yaw
+ << " and pitch=" << pitch;
- const float currentYawVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headYawJoint->getName()))->getFloat();
- const float currentPitchVel = DatafieldRefPtr::dynamicCast(kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities", headPitchJoint->getName()))->getFloat();
+ const float currentYawVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headYawJoint->getName()))->getFloat();
+ const float currentPitchVel = DatafieldRefPtr::dynamicCast(
+ kinematicUnitObserverInterfacePrx->getDatafieldRefByName("jointvelocities",
+ headPitchJoint->getName()))->getFloat();
FrameTracking::HeadState headState;
headState.currentYawPos = currentYaw;
@@ -524,41 +591,49 @@ namespace armarx
return headState;
}
- void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
+ void FrameTracking::_doVelocityControl(const FrameTracking::HeadState& headState, float maxYawVelocity,
+ float yawAcceleration, float maxPitchVelocity, float pitchAcceleration)
{
if (headState.stop)
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
return;
}
- float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentYawVel, maxYawVelocity,
- yawAcceleration, yawAcceleration,
- headState.currentYawPos, headState.desiredYawPos,
- 1.f);
- float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(
- 30.f / 1000, 35.f / 1000,
- headState.currentPitchVel, maxPitchVelocity,
- pitchAcceleration, pitchAcceleration,
- headState.currentPitchPos, headState.desiredPitchPos,
- 1.f);
+ float desiredYawVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
+ headState.currentYawVel,
+ maxYawVelocity, yawAcceleration,
+ yawAcceleration,
+ headState.currentYawPos,
+ headState.desiredYawPos, 1.f);
+ float desiredPitchVelocity = positionThroughVelocityControlWithAccelerationBounds(30.f / 1000, 35.f / 1000,
+ headState.currentPitchVel,
+ maxPitchVelocity,
+ pitchAcceleration,
+ pitchAcceleration,
+ headState.currentPitchPos,
+ headState.desiredPitchPos,
+ 1.f);
// control mode is set when enable task
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity}, {headPitchJoint->getName(), desiredPitchVelocity}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), desiredYawVelocity},
+ {headPitchJoint->getName(), desiredPitchVelocity}});
}
void FrameTracking::_doPositionControl(const FrameTracking::HeadState& headState)
{
auto currentModes = kinematicUnitInterfacePrx->getControlModes();
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl}, {headPitchJoint->getName(), ControlMode::ePositionControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::ePositionControl},
+ {headPitchJoint->getName(), ControlMode::ePositionControl}});
if (headState.stop)
{
return;
}
- kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos}, {headPitchJoint->getName(), headState.desiredPitchPos}});
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]}, {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
+ kinematicUnitInterfacePrx->setJointAngles({{headYawJoint->getName(), headState.desiredYawPos},
+ {headPitchJoint->getName(), headState.desiredPitchPos}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), currentModes[headYawJoint->getName()]},
+ {headPitchJoint->getName(), currentModes[headPitchJoint->getName()]}});
}
void FrameTracking::_enableTracking(bool enable)
@@ -570,12 +645,13 @@ namespace armarx
this->enabled = enable;
if (enable)
{
- kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl}, {headPitchJoint->getName(), ControlMode::eVelocityControl}});
+ kinematicUnitInterfacePrx->switchControlMode({{headYawJoint->getName(), ControlMode::eVelocityControl},
+ {headPitchJoint->getName(), ControlMode::eVelocityControl}});
processorTask->start();
- }
- else
+ } else
{
- kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f}, {headPitchJoint->getName(), 0.f}});
+ kinematicUnitInterfacePrx->setJointVelocities({{headYawJoint->getName(), 0.f},
+ {headPitchJoint->getName(), 0.f}});
processorTask->stop();
}
}
diff --git a/source/RobotAPI/components/FrameTracking/FrameTracking.h b/source/RobotAPI/components/FrameTracking/FrameTracking.h
index d0d76bba2..1ba272f3b 100644
--- a/source/RobotAPI/components/FrameTracking/FrameTracking.h
+++ b/source/RobotAPI/components/FrameTracking/FrameTracking.h
@@ -149,6 +149,7 @@ namespace armarx
void lookAtFrame(const std::string& frameName, const Ice::Current& = Ice::emptyCurrent) override;
void lookAtPointInGlobalFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
+ bool isLookingAtPointInGlobalFrame(const Vector3f& point, float max_diff, const Ice::Current& = Ice::emptyCurrent) override;
void lookAtPointInRobotFrame(const Vector3f& point, const Ice::Current& = Ice::emptyCurrent) override;
void scanInConfigurationSpace(float yawFrom, float yawTo, const ::Ice::FloatSeq& pitchValues, float velocity, const Ice::Current& = Ice::emptyCurrent) override;
@@ -161,6 +162,7 @@ namespace armarx
void syncronizeLocalClone();
void _lookAtFrame(const std::string& frame);
void _lookAtPoint(const Eigen::Vector3f& point);
+ bool _looksAtPoint(const Eigen::Vector3f& point, float max_diff);
HeadState _calculateJointAnglesContinously(const std::string& frame);
HeadState _calculateJointAngles(const Eigen::Vector3f& point);
void _doVelocityControl(const HeadState& headstate, float maxYawVelocity, float yawAcceleration, float maxPitchVelocity, float pitchAcceleration);
diff --git a/source/RobotAPI/interface/components/FrameTrackingInterface.ice b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
index 60bcb8a35..ba0434110 100644
--- a/source/RobotAPI/interface/components/FrameTrackingInterface.ice
+++ b/source/RobotAPI/interface/components/FrameTrackingInterface.ice
@@ -36,6 +36,7 @@ module armarx
void lookAtFrame(string frameName);
void lookAtPointInGlobalFrame(Vector3f point);
+ bool isLookingAtPointInGlobalFrame(Vector3f point, float max_diff);
void lookAtPointInRobotFrame(Vector3f point);
void scanInConfigurationSpace(float yawFrom, float yawTo, ::Ice::FloatSeq pitchValues, float velocity);
--
GitLab