Skip to content
Snippets Groups Projects
Commit f03fb7f8 authored by Peter Kaiser's avatar Peter Kaiser
Browse files

TCPControlUnitObserver: Fixed equalsPoseWithTol check

parent 05d0119c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/components/units/TCPControlUnitObserver.cpp
+ 7
− 2
View file @ f03fb7f8
......@@ -29,6 +29,7 @@
#include <Core/observers/checks/ConditionCheckInRange.h>
#include <Core/observers/checks/ConditionCheckLarger.h>
#include <Core/observers/checks/ConditionCheckSmaller.h>
#include <RobotAPI/libraries/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h>
#include <RobotAPI/libraries/robotstate/remote/checks/ConditionCheckMagnitudeChecks.h>
#include <Core/core/exceptions/local/ExpressionException.h>
......@@ -52,7 +53,7 @@ namespace armarx {
offerConditionCheck("smaller", new ConditionCheckSmaller());
// offerConditionCheck("magnitudeinrange", new ConditionCheckInRange());
offerConditionCheck("magnitudelarger", new ConditionCheckMagnitudeLarger());
// offerConditionCheck("equalsPoseWithTol", new ConditionCheckEqualsPoseWithTolerance());
offerConditionCheck("equalsPoseWithTol", new ConditionCheckEqualsPoseWithTolerance());
offerChannel(TCP_POSE_CHANNEL, "TCP poses of the robot.");
......@@ -74,6 +75,7 @@ namespace armarx {
void TCPControlUnitObserver::reportTCPPose(const FramedPoseBaseMap &poseMap, const Ice::Current &)
{
ScopedLock lock(dataMutex);
ARMARX_INFO << deactivateSpam() << "new tcp poses reported";
FramedPoseBaseMap::const_iterator it = poseMap.begin();
for(; it != poseMap.end(); it++)
{
......@@ -84,7 +86,7 @@ namespace armarx {
offerDataFieldWithDefault(TCP_POSE_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
else
setDataField(TCP_POSE_CHANNEL, tcpName, Variant(it->second));
updateChannel(TCP_POSE_CHANNEL);
if(!existsChannel(tcpName))
{
offerChannel(tcpName, "pose components of " + tcpName);
......@@ -108,6 +110,7 @@ namespace armarx {
setDataField(tcpName,"qw", Variant(vec->orientation->qw));
setDataField(tcpName,"frame", Variant(vec->frame));
}
updateChannel(tcpName);
}
}
......@@ -132,6 +135,7 @@ namespace armarx {
offerDataFieldWithDefault(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second), "Pose of " + tcpName);
else
setDataField(TCP_TRANS_VELOCITIES_CHANNEL, tcpName, Variant(it->second));
updateChannel(TCP_TRANS_VELOCITIES_CHANNEL);
const std::string channelName = tcpName+"Velocities";
if(!existsChannel(channelName))
{
......@@ -154,6 +158,7 @@ namespace armarx {
setDataField(channelName,"yaw", Variant(vecOri->z));
setDataField(channelName,"frame", Variant(vec->frame));
}
updateChannel(channelName);
}
}
......
source/RobotAPI/libraries/robotstate/remote/checks/ConditionCheckEqualsPoseWithTolerance.h
+ 19
− 17
View file @ f03fb7f8
......@@ -3,7 +3,7 @@
#include <Core/core/system/ImportExport.h>
#include <Core/observers/ConditionCheck.h>
#include <Core/robotstate/remote/ArmarPose.h>
#include <RobotAPI/libraries/robotstate/remote/ArmarPose.h>
namespace armarx
{
......@@ -18,7 +18,7 @@ namespace armarx
addSupportedType(VariantType::FramedOrientation, createParameterTypeList(2, VariantType::FramedOrientation, VariantType::Float));
addSupportedType(VariantType::Vector3, createParameterTypeList(2, VariantType::Vector3, VariantType::Float));
addSupportedType(VariantType::Quaternion, createParameterTypeList(2, VariantType::Quaternion, VariantType::Float));
addSupportedType(VariantType::Pose, createParameterTypeList(2, VariantType::Pose, VariantType::Float));
addSupportedType(VariantType::FramedPose, createParameterTypeList(3, VariantType::FramedPose, VariantType::Float, VariantType::Float));
}
ConditionCheck* clone()
......@@ -34,13 +34,13 @@ namespace armarx
throw InvalidConditionException("Wrong number of datafields for condition equals ");
}
Variant& value = dataFields.begin()->second;
const Variant& value = dataFields.begin()->second;
VariantTypeId type = value.getType();
if(type == VariantType::Vector3)
{
Vector3Ptr & typedValue = value.getClass<Vector3>();
Vector3Ptr & param = getParameter(0).getClass<Vector3>();
const Vector3Ptr & typedValue = value.getClass<Vector3>();
const Vector3Ptr & param = getParameter(0).getClass<Vector3>();
return (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
((typedValue->y-param->y)*(typedValue->y-param->y))+
((typedValue->z-param->x)*(typedValue->x-param->z))) < getParameter(1).getFloat());
......@@ -48,8 +48,8 @@ namespace armarx
if(type == VariantType::Quaternion)
{
QuaternionPtr & typedValue = value.getClass<Quaternion>();
QuaternionPtr & param = getParameter(0).getClass<Quaternion>();
const QuaternionPtr & typedValue = value.getClass<Quaternion>();
const QuaternionPtr & param = getParameter(0).getClass<Quaternion>();
return (sqrt(((typedValue->qw-param->qw)*(typedValue->qw-param->qw))+
((typedValue->qx-param->qx)*(typedValue->qx-param->qx))+
((typedValue->qy-param->qy)*(typedValue->qy-param->qy))+
......@@ -59,8 +59,8 @@ namespace armarx
if(type == VariantType::FramedPosition)
{
FramedPositionPtr & typedValue = value.getClass<FramedPosition>();
FramedPositionPtr & param = getParameter(0).getClass<FramedPosition>();
const FramedPositionPtr & typedValue = value.getClass<FramedPosition>();
const FramedPositionPtr & param = getParameter(0).getClass<FramedPosition>();
return param->getFrame() == typedValue->getFrame()
&& (sqrt(((typedValue->x-param->x)*(typedValue->x-param->x))+
((typedValue->y-param->y)*(typedValue->y-param->y))+
......@@ -70,8 +70,8 @@ namespace armarx
if(type == VariantType::FramedOrientation)
{
FramedOrientationPtr & typedValue = value.getClass<FramedOrientation>();
FramedOrientationPtr & param = getParameter(0).getClass<FramedOrientation>();
const FramedOrientationPtr & typedValue = value.getClass<FramedOrientation>();
const FramedOrientationPtr & param = getParameter(0).getClass<FramedOrientation>();
return param->getFrame() == typedValue->getFrame()
&&(sqrt(((typedValue->qw-param->qw)*(typedValue->qw-param->qw))+
((typedValue->qx-param->qx)*(typedValue->qx-param->qx))+
......@@ -82,16 +82,18 @@ namespace armarx
if(type == VariantType::Pose)
{
// TODO: This comparison is bullshit!
PosePtr & typedValue = value.getClass<Pose>();
PosePtr & param = getParameter(0).getClass<Pose>();
return (sqrt(((typedValue->position->x-param->position->x)*(typedValue->position->x-param->position->x))+
const PosePtr & typedValue = value.getClass<FramedPose>();
const PosePtr & param = getParameter(0).getClass<FramedPose>();
bool positionOk = (sqrt(((typedValue->position->x-param->position->x)*(typedValue->position->x-param->position->x))+
((typedValue->position->y-param->position->y)*(typedValue->position->y-param->position->y))+
((typedValue->position->z-param->position->x)*(typedValue->position->x-param->position->z)))+
((typedValue->position->z-param->position->x)*(typedValue->position->x-param->position->z)))
< getParameter(1).getFloat());
bool orientationOk =
sqrt(((typedValue->orientation->qw-param->orientation->qw)*(typedValue->orientation->qw-param->orientation->qw))+
((typedValue->orientation->qx-param->orientation->qx)*(typedValue->orientation->qx-param->orientation->qx))+
((typedValue->orientation->qy-param->orientation->qy)*(typedValue->orientation->qy-param->orientation->qy))+
((typedValue->orientation->qz-param->orientation->qx)*(typedValue->orientation->qx-param->orientation->qz))) < getParameter(1).getFloat());
((typedValue->orientation->qz-param->orientation->qx)*(typedValue->orientation->qx-param->orientation->qz))) < getParameter(2).getFloat();
return positionOk && orientationOk;
}
return false;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment