Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • sw/armarx/robot-components
1 result
Show changes
Commits on Source (17)
Hide whitespace changes
Inline Side-by-side
Showing
with 291 additions and 132 deletions
CMakeLists.txt
View file @ 1dc19d03
...@@ -21,6 +21,9 @@ depends_on_armarx_package(MemoryX "OPTIONAL") ...@@ -21,6 +21,9 @@ depends_on_armarx_package(MemoryX "OPTIONAL")
find_package(Eigen3 QUIET) find_package(Eigen3 QUIET)
find_package(Simox QUIET) find_package(Simox QUIET)
if (Simox_FOUND)
setupSimoxExternalLibraries()
endif()
add_subdirectory(etc) add_subdirectory(etc)
add_subdirectory(source) add_subdirectory(source)
......
data/RobotComponents/VariantInfo-RobotComponents.xml
View file @ 1dc19d03
...@@ -9,6 +9,22 @@ ...@@ -9,6 +9,22 @@
propertyName="ViewSelectionName" propertyName="ViewSelectionName"
propertyIsOptional="true" propertyIsOptional="true"
propertyDefaultValue="ViewSelection" /> propertyDefaultValue="ViewSelection" />
<Proxy include="RobotComponents/interface/components/RobotIK.h"
humanName="RobotIK"
typeName="RobotIKInterfacePrx"
memberName="robotIK"
getterName="getRobotIK"
propertyName="RobotIKName"
propertyIsOptional="true"
propertyDefaultValue="RobotIK" />
<Proxy include="RobotComponents/interface/components/DMPComponentBase.h"
humanName="DMP Control Component"
typeName="DMPComponentBasePrx"
memberName="dmpComponent"
getterName="getDMPComponent"
propertyName="DMPComponentName"
propertyIsOptional="true"
propertyDefaultValue="DMPComponent" />
<!-- <Proxy include="RobotComponents/interface/components/PathPlanner.h" <!-- <Proxy include="RobotComponents/interface/components/PathPlanner.h"
humanName="PathPlanner" humanName="PathPlanner"
typeName="ViewSelectionInterfacePrx" typeName="ViewSelectionInterfacePrx"
......
etc/cmake/ArmarXPackageVersion.cmake
View file @ 1dc19d03
# armarx version file # armarx version file
set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0") set(ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "7") set(ARMARX_PACKAGE_LIBRARY_VERSION_MINOR "8")
set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "7") set(ARMARX_PACKAGE_LIBRARY_VERSION_PATCH "0")
set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}") set(ARMARX_PACKAGE_LIBRARY_VERSION "${ARMARX_PACKAGE_LIBRARY_VERSION_MAJOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_MINOR}.${ARMARX_PACKAGE_LIBRARY_VERSION_PATCH}")
......
etc/doxygen/pages/__HowTos.dox__
View file @ 1dc19d03
/**
\page RobotComponents-HowTos RobotComponents HowTos \page RobotComponents-HowTos RobotComponents HowTos
\section RobotComponents-HowTo-change_this HowTo Title \section RobotComponents-HowTo-ViewSelection How to use the Automatic View Selection
\subsection The automatic mode
The purpose of the automatic view selection is to make the robot autonomously look in the most useful direction at any time. If, for example, the current task is to grasp an object, and during the grasping both the object and the hand should be localized visually, the robot should look at them both at once if possible, or alternatingly otherwise.
This is achieved by considering the localization uncertainties of the requested objects in the working memory. The priority of an object to be looked at is the logarithm of the volume of its uncertainty ellipsoid. The Automatic View Selection will find the view direction that maximizes the sum of the priorities of all objects that are visible in that direction. Details on this can be found in the following paper:
K. Welke, D. Schiebener, T. Asfour and R. Dillmann: Gaze selection during manipulation tasks, IEEE International Conference on Robotics and Automation (ICRA), 2013
Additionally, there is a little random component that slightly modifies the view direction, which is helpful when anobject is at the edge of the field of view or the localization only succeeds from time to time. Empiriclly, this leads to somewhat more stable localization results. The effect of this random component is most visible when no object has been requested and the robot just looks around without purpose.
A new view direction is by default chosen every 2.5 seconds (this can be changed in the config of the ViewSelection component). This gives the robot enough time to move the head to the target configuration, get to rest there so the camera images are not blurry, and let localizers run a few times.
The default maximal distance in which an object is considered to be localizable is 1.5 meters. If the object is further away, the view selection will ignore it. Other important parameters that you might need to adapt for your robot are the camera opening angle and the range of motion for the head. The default parameters are set for ARMAR-III.
\subsection Setting manual view targets
Description (the change_this part of the section identifier must be edited to be descriptive and unique) Most of the time, the fully automatic mode should work fine for your purposes. As long as the robot knows where the objects of interest are, it will look into the direction with the highest accumulated uncertainty of relevant objects. However, if the robot doesn't know where an object is, e.g. because it has not been localized yet, you may need to set a view direction manually. You can do this with the method addManualViewTarget(const FramedPositionBasePtr& target).
When you set a manual view target, the next time a new view direction is set, it will be towards the position you requested. You can add several manual targets, they will be looked at in the order in which you added them in the usual 2.5 second interval. Afterwards, if the automatic view selection is activated, it will continue normally. If it is deactivated, your manual targets will be looked at ,and then the robot will not change the view direction further after the last one.
*/ In most cases, the best strategy is to set the expected possible object locations as manual view targets in the beginning, and let the view selection run automatically otherwise. You should check at key times in your statechart whether the required objects are successfully being localized (the standard skills like visual servoing and grasping do that), and if not, add manual view targets to find them.
source/RobotComponents/components/DMPComponent/DMPComponent.cpp
View file @ 1dc19d03
...@@ -23,7 +23,7 @@ ...@@ -23,7 +23,7 @@
#include "DMPComponent.h" #include "DMPComponent.h"
#include <MMM/Motion/MotionReaderXML.h> #include <MMM/Motion/MotionReaderXML.h>
#include <dmp/io/MMMConverter.h> #include <dmp/io/MMMConverter.h>
#include <dmp/testing/testdataset.h>
using namespace armarx; using namespace armarx;
...@@ -106,15 +106,17 @@ void DMPComponent::instantiateDMP(int DMPType, const Ice::Current&) ...@@ -106,15 +106,17 @@ void DMPComponent::instantiateDMP(int DMPType, const Ice::Current&)
case ARMARX_DMPTYPE_SIMPLEENDVELODMP: case ARMARX_DMPTYPE_SIMPLEENDVELODMP:
basicdmp.reset(new DMP::SimpleEndVeloDMP); basicdmp.reset(new DMP::SimpleEndVeloDMP);
break; break;
// case ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP: basicdmp.reset(new DMP::EndVeloForceFieldDMP);break; // case ARMARX_DMPTYPE_ENDVELFORCEFILELDDMP: basicdmp.reset(new DMP::EndVeloForceFieldDMP);break;
// case ARMARX_DMPTYPE_QUATERNIONDMP: basicdmp.reset(new DMP::QuaternionDMP); break; //error: quanternionDMP is an abstract type. // case ARMARX_DMPTYPE_QUATERNIONDMP: basicdmp.reset(new DMP::QuaternionDMP); break; //error: quanternionDMP is an abstract type.
// case ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP: basicdmp.reset(new DMP::AdaptiveGoal3rdOrderDMP); break; //error: // case ARMARX_DMPTYPE_ADAPTIVEGOAL3RDORDERDMP: basicdmp.reset(new DMP::AdaptiveGoal3rdOrderDMP); break; //error:
// case ARMARX_DMPTYPE_PERIODICTRANSIENTDMP: basicdmp.reset(new DMP::PeriodicTransientDMP); break; case ARMARX_DMPTYPE_PERIODICTRANSIENTDMP:
ARMARX_INFO << "instantiate PeriodicDMP";
basicdmp.reset(new DMP::PeriodicTransientDMP);
break;
default: default:
ARMARX_ERROR << "ERROR: It is not a valid dmp type. " ; ARMARX_ERROR << "ERROR: It is not a valid dmp type. " ;
} }
tCurrent = 0;
} }
...@@ -198,14 +200,24 @@ void DMPComponent::getDMPFromDatabase(const std::string& dmpName, const Ice::Cur ...@@ -198,14 +200,24 @@ void DMPComponent::getDMPFromDatabase(const std::string& dmpName, const Ice::Cur
void DMPComponent::trainDMP(const ::Ice::Current&) void DMPComponent::trainDMP(const ::Ice::Current&)
{ {
// learn dmp // learn dmp
if (getBasicDMPType() == ARMARX_DMPTYPE_BASICDMP) ARMARX_INFO << "In Train DMP";
if (dmpType == ARMARX_DMPTYPE_BASICDMP)
{ {
basicdmp->learnFromTrajectories(trajs); basicdmp->learnFromTrajectories(trajs);
} }
else if (getBasicDMPType() == ARMARX_DMPTYPE_DMP3RDORDER) else if (dmpType == ARMARX_DMPTYPE_DMP3RDORDER)
{ {
dmp3rdorder->learnFromTrajectories(trajs); dmp3rdorder->learnFromTrajectories(trajs);
} }
else if (dmpType == ARMARX_DMPTYPE_PERIODICTRANSIENTDMP)
{
basicdmp->learnFromTrajectories(trajs);
}
ARMARX_INFO << "Exit Train DMP";
} }
void DMPComponent::calculateWholeTrajectory() void DMPComponent::calculateWholeTrajectory()
...@@ -279,6 +291,33 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu ...@@ -279,6 +291,33 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu
DMP::SampledTrajectoryV2 traj; DMP::SampledTrajectoryV2 traj;
traj.readFromCSVFile(file); traj.readFromCSVFile(file);
trajs.push_back(traj); trajs.push_back(traj);
}
else if (ext == "vsg")
{
trajs.clear();
TestDataSet set;
set.readFromFile(file);
set.cutConstantBeginning();
DMP::SampledTrajectoryV2 testTraj;//= set.trajectoryForPositionsV2();
trajs.push_back(testTraj);
set.trajectoryForPositionsV2(trajs[0], usedDimensions);
//DVec anchorPoint;//delete set;
//anchorPoint.push_back(0.0);
//anchorPoint.push_back(1.0);
//anchorPoint.push_back(2.0);
//trajs[0].m_anchorPoint = anchorPoint;
ARMARX_INFO << "VSG File LOADED";
//trajs.resize(1);
//trajs[1] = traji;
ARMARX_INFO << "VSG File LOADED";
printf("Size of traj %f %f\n", trajs[0].m_anchorPoint[0], trajs[0].m_anchorPoint[1]);
ARMARX_INFO << "VSG File LOADED";
//ARMARX_INFO << "Size of loaded Trajectories " << trajs.size() << " and " << traj.getAnchorPoint();
ARMARX_INFO << "VSG File LOADED DONE";
} }
else else
{ {
...@@ -288,7 +327,6 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu ...@@ -288,7 +327,6 @@ void DMPComponent::readTrajectoryFromFile(const std::string& file, const Ice::Cu
} }
void DMPComponent::setDMPConfiguration() void DMPComponent::setDMPConfiguration()
{ {
...@@ -438,6 +476,7 @@ void DMPComponent::setTimeStamps(const DVector& value, const Ice::Current&) ...@@ -438,6 +476,7 @@ void DMPComponent::setTimeStamps(const DVector& value, const Ice::Current&)
void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&) void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
{ {
canonicalValues = DMP::DVec(value); canonicalValues = DMP::DVec(value);
} }
...@@ -473,21 +512,21 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&) ...@@ -473,21 +512,21 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
::armarx::cStateVec DMPComponent::getNextState(const ::Ice::Current&) ::armarx::cStateVec DMPComponent::getNextState(const ::Ice::Current&)
{ {
ARMARX_INFO << "In getNext State ";
setDMPConfiguration(); setDMPConfiguration();
if (timestamps.size() == 0) if (timestamps.size() == 0)
{ {
ARMARX_ERROR << "Timestampes must be specified."; ARMARX_ERROR << "Timestampes must be specified.";
} }
ARMARX_INFO << "In getNext State 1";
if (canonicalValues.size() == 0) if (canonicalValues.size() == 0)
{ {
ARMARX_WARNING << "Canonical value is not specified. It will be set 1.0."; ARMARX_WARNING << "Canonical value is not specified. It will be set 1.0.";
canonicalValues.push_back(1.0); canonicalValues.push_back(1.0);
} }
ARMARX_INFO << "In getNext State 2";
double tInit = timestamps[tCurrent++]; double tInit = timestamps[tCurrent++];
...@@ -506,7 +545,7 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&) ...@@ -506,7 +545,7 @@ void DMPComponent::setCanonicalValues(const DVector& value, const Ice::Current&)
tCurrent = 0; tCurrent = 0;
} }
ARMARX_INFO << "In getNext State 3";
double t = timestamps[tCurrent]; double t = timestamps[tCurrent];
...@@ -552,6 +591,15 @@ void DMPComponent::removeDMPFromDatabase(const std::string& dmpName, const ::Ice ...@@ -552,6 +591,15 @@ void DMPComponent::removeDMPFromDatabase(const std::string& dmpName, const ::Ice
} }
} }
void DMPComponent::setDimensionsToLearn(const DVector& value, const Ice::Current&)
{
usedDimensions.clear();
for (int i = 0; i < value.size(); i++)
{
usedDimensions.push_back(int(value[i]));
}
}
//PropertyDefinitionsPtr DMPComponent::createPropertyDefinitions() //PropertyDefinitionsPtr DMPComponent::createPropertyDefinitions()
//{ //{
......
source/RobotComponents/components/DMPComponent/DMPComponent.h
View file @ 1dc19d03
...@@ -54,7 +54,7 @@ ...@@ -54,7 +54,7 @@
#include "dmp/representation/dmp/simpleendvelodmp.h" #include "dmp/representation/dmp/simpleendvelodmp.h"
//#include "dmp/representation/dmp/endveloforcefielddmp.h" //#include "dmp/representation/dmp/endveloforcefielddmp.h"
//#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h" //#include "dmp/representation/dmp/endveloforcefieldwithobjrepulsiondmp.h"
//#include "dmp/representation/dmp/periodictransientdmp.h" #include "dmp/representation/dmp/periodictransientdmp.h"
namespace armarx namespace armarx
{ {
...@@ -113,6 +113,7 @@ namespace armarx ...@@ -113,6 +113,7 @@ namespace armarx
typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::DMPState> > BasicDMPInterfacePtr; typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::DMPState> > BasicDMPInterfacePtr;
typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::_3rdOrderDMP> > DMP3rdOrderInterfacePtr; typedef boost::shared_ptr<DMP::DMPInterfaceTemplate<DMP::_3rdOrderDMP> > DMP3rdOrderInterfacePtr;
DMPComponent(): DMPComponent():
basicdmp(new DMP::BasicDMP()), basicdmp(new DMP::BasicDMP()),
dmp3rdorder(new DMP::DMP3rdOrder()), dmp3rdorder(new DMP::DMP3rdOrder()),
...@@ -168,7 +169,7 @@ namespace armarx ...@@ -168,7 +169,7 @@ namespace armarx
virtual void trainDMP(const ::Ice::Current& = ::Ice::Current()); virtual void trainDMP(const ::Ice::Current& = ::Ice::Current());
virtual void storeDMPInDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current()); virtual void storeDMPInDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current());
virtual void removeDMPFromDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current()); virtual void removeDMPFromDatabase(const std::string& name, const ::Ice::Current& = ::Ice::Current());
virtual void setDimensionsToLearn(const DVector& value, const Ice::Current& = ::Ice::Current());
//transmit data from server to client (using ice) //transmit data from server to client (using ice)
...@@ -203,9 +204,11 @@ namespace armarx ...@@ -203,9 +204,11 @@ namespace armarx
DMP::DVec timestamps; DMP::DVec timestamps;
DMP::DVec canonicalValues; DMP::DVec canonicalValues;
int tCurrent; int tCurrent;
std::vector<int> usedDimensions;
protected: protected:
BasicDMPInterfacePtr basicdmp; BasicDMPInterfacePtr basicdmp;
DMP3rdOrderInterfacePtr dmp3rdorder; DMP3rdOrderInterfacePtr dmp3rdorder;
DMP::Vec<DMP::SampledTrajectoryV2> trajs; DMP::Vec<DMP::SampledTrajectoryV2> trajs;
int dmpType; int dmpType;
......
source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp
View file @ 1dc19d03
...@@ -40,8 +40,8 @@ void MMMPlayer::onInitComponent() ...@@ -40,8 +40,8 @@ void MMMPlayer::onInitComponent()
usingProxy(getProperty<std::string>("KinematicUnitName").getValue()); usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("DebugObserver"); usingProxy("DebugObserver");
motionFPS = 1; motionFPS = 1.0f;
desiredFPS = 1; desiredFPS = 1.0f;
frameOffset = 0; frameOffset = 0;
paused = true; paused = true;
...@@ -50,16 +50,15 @@ void MMMPlayer::onInitComponent() ...@@ -50,16 +50,15 @@ void MMMPlayer::onInitComponent()
desiredFPS = getProperty<float>("FPS").getValue(); desiredFPS = getProperty<float>("FPS").getValue();
maxVel = getProperty<float>("absMaximumVelocity").getValue(); maxVel = getProperty<float>("absMaximumVelocity").getValue();
if (usePIDController)
{ usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
}
} }
void MMMPlayer::onConnectComponent() void MMMPlayer::onConnectComponent()
{ {
kin = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue()); kinematicUnit = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver"); debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue(); std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue();
...@@ -68,7 +67,7 @@ void MMMPlayer::onConnectComponent() ...@@ -68,7 +67,7 @@ void MMMPlayer::onConnectComponent()
std::vector<std::string> projects; std::vector<std::string> projects;
boost::split(projects, armarxProjects, boost::is_any_of(",;"), boost::token_compress_on); boost::split(projects, armarxProjects, boost::is_any_of(",;"), boost::token_compress_on);
for (auto & p : projects) for (std::string & p : projects)
{ {
// ARMARX_INFO << "Adding to datapaths of " << p; // ARMARX_INFO << "Adding to datapaths of " << p;
armarx::CMakePackageFinder finder(p); armarx::CMakePackageFinder finder(p);
...@@ -85,9 +84,9 @@ void MMMPlayer::onConnectComponent() ...@@ -85,9 +84,9 @@ void MMMPlayer::onConnectComponent()
} }
} }
auto motionDefault = getProperty<std::string>("MMMFile").getValue(); std::string motionDefault = getProperty<std::string>("MMMFile").getValue();
if (motionDefault != "") if (!motionDefault.empty())
{ {
load(motionDefault, "Armar4"); load(motionDefault, "Armar4");
} }
...@@ -148,8 +147,7 @@ bool MMMPlayer::pauseMMMPlayer(const Ice::Current&) ...@@ -148,8 +147,7 @@ bool MMMPlayer::pauseMMMPlayer(const Ice::Current&)
if (!paused) if (!paused)
{ {
paused = true; paused = true;
kin->setJointVelocities(nullVelocities); kinematicUnit->setJointVelocities(nullVelocities);
sleep(1);
if (direction < 0) if (direction < 0)
{ {
...@@ -194,7 +192,7 @@ bool MMMPlayer::stopMMMPlayer(const Ice::Current&) ...@@ -194,7 +192,7 @@ bool MMMPlayer::stopMMMPlayer(const Ice::Current&)
ARMARX_INFO << "stopped MMMPlayer task from GUI"; ARMARX_INFO << "stopped MMMPlayer task from GUI";
} }
kin->setJointVelocities(nullVelocities); kinematicUnit->setJointVelocities(nullVelocities);
return !(task->isRunning()); return !(task->isRunning());
} }
...@@ -224,7 +222,7 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects) ...@@ -224,7 +222,7 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
std::vector<std::string> proj; std::vector<std::string> proj;
boost::split(proj, projects, boost::is_any_of(",;"), boost::token_compress_on); boost::split(proj, projects, boost::is_any_of(",;"), boost::token_compress_on);
for (auto & p : proj) for (std::string & p : proj)
{ {
ARMARX_INFO << "Adding to datapaths of " << p; ARMARX_INFO << "Adding to datapaths of " << p;
armarx::CMakePackageFinder finder(p); armarx::CMakePackageFinder finder(p);
...@@ -399,8 +397,8 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects) ...@@ -399,8 +397,8 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
jointNamesUsed[jointName] = true; jointNamesUsed[jointName] = true;
} }
kin->switchControlMode(modes); kinematicUnit->switchControlMode(modes);
kin->setJointVelocities(nullVelocities); kinematicUnit->setJointVelocities(nullVelocities);
} }
bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& projects, const Ice::Current&) bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& projects, const Ice::Current&)
...@@ -420,6 +418,7 @@ bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& pr ...@@ -420,6 +418,7 @@ bool MMMPlayer::loadTrajectory(const std::string& MMMFile, const std::string& pr
int MMMPlayer::getNumberOfFrames(const Ice::Current&) int MMMPlayer::getNumberOfFrames(const Ice::Current&)
{ {
if (!motion) if (!motion)
{ {
return 0; return 0;
...@@ -435,6 +434,7 @@ int MMMPlayer::getCurrentFrame(const Ice::Current&) ...@@ -435,6 +434,7 @@ int MMMPlayer::getCurrentFrame(const Ice::Current&)
bool MMMPlayer::setLoopPlayback(bool loop, const Ice::Current&) bool MMMPlayer::setLoopPlayback(bool loop, const Ice::Current&)
{ {
loopPlayback = loop; loopPlayback = loop;
return loopPlayback; return loopPlayback;
} }
...@@ -449,8 +449,11 @@ StringList MMMPlayer::getJointNames(const Ice::Current&) ...@@ -449,8 +449,11 @@ StringList MMMPlayer::getJointNames(const Ice::Current&)
return jointNames; return jointNames;
} }
int MMMPlayer::setUsePID(int usePID, const Ice::Current&) int MMMPlayer::setUsePID(bool usePID, const Ice::Current&)
{ {
ScopedRecursiveLock lock(mmmMutex);
usePIDController = usePID; usePIDController = usePID;
NameControlModeMap modes; NameControlModeMap modes;
...@@ -470,14 +473,14 @@ int MMMPlayer::setUsePID(int usePID, const Ice::Current&) ...@@ -470,14 +473,14 @@ int MMMPlayer::setUsePID(int usePID, const Ice::Current&)
try try
{ {
kin->switchControlMode(modes); kinematicUnit->switchControlMode(modes);
} }
catch (...) { } catch (...) { }
return usePIDController; return usePIDController;
} }
void MMMPlayer::setFPS(int FPS, const Ice::Current&) void MMMPlayer::setFPS(float FPS, const Ice::Current&)
{ {
desiredFPS = FPS; desiredFPS = FPS;
ARMARX_VERBOSE << "set FPS to " << FPS; ARMARX_VERBOSE << "set FPS to " << FPS;
...@@ -492,6 +495,9 @@ bool MMMPlayer::setJointsInUse(const std::string& jointName, bool inUse, const I ...@@ -492,6 +495,9 @@ bool MMMPlayer::setJointsInUse(const std::string& jointName, bool inUse, const I
void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&) void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
{ {
ScopedRecursiveLock lock(mmmMutex);
if (!motion) if (!motion)
{ {
return; return;
...@@ -534,15 +540,14 @@ void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&) ...@@ -534,15 +540,14 @@ void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
} }
debugObserver->setDebugChannel("targetJointAngles", debugTargetValues); debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
kin->setJointVelocities(nullVelocities); kinematicUnit->setJointVelocities(nullVelocities);
kin->setJointAngles(targetPositionValues); kinematicUnit->setJointAngles(targetPositionValues);
ARMARX_INFO << "MMMPlayer: Current frame set to " << frameNumber; ARMARX_INFO << "MMMPlayer: Current frame set to " << frameNumber;
} }
void MMMPlayer::run() void MMMPlayer::run()
{ {
NameValueMap targetVelocities; NameValueMap targetVelocities;
if (!motion) if (!motion)
...@@ -574,7 +579,7 @@ void MMMPlayer::run() ...@@ -574,7 +579,7 @@ void MMMPlayer::run()
{ {
ARMARX_VERBOSE << "current frame " << currentFrame; ARMARX_VERBOSE << "current frame " << currentFrame;
kin->setJointVelocities(nullVelocities); kinematicUnit->setJointVelocities(nullVelocities);
if (loopPlayback) if (loopPlayback)
{ {
...@@ -609,6 +614,7 @@ void MMMPlayer::run() ...@@ -609,6 +614,7 @@ void MMMPlayer::run()
ARMARX_INFO << deactivateSpam(1) << "frame: " << currentFrame << " " << VAROUT(interpolationValue); ARMARX_INFO << deactivateSpam(1) << "frame: " << currentFrame << " " << VAROUT(interpolationValue);
ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows()); ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows());
for (size_t i = 0; i < jointNames.size(); ++i) for (size_t i = 0; i < jointNames.size(); ++i)
{ {
const auto& jointName = jointNames.at(i); const auto& jointName = jointNames.at(i);
...@@ -629,7 +635,7 @@ void MMMPlayer::run() ...@@ -629,7 +635,7 @@ void MMMPlayer::run()
targetPosValue += it->second; targetPosValue += it->second;
} }
assert(targetPosValue == targetPositionValues[jointName]); // assert(targetPosValue == targetPositionValues[jointName]);
debugTargetValues[jointName] = new Variant(targetPosValue); debugTargetValues[jointName] = new Variant(targetPosValue);
float& targetVel = targetVelocities[jointName] = 0.0; float& targetVel = targetVelocities[jointName] = 0.0;
auto vel = frame->joint_vel[i]; auto vel = frame->joint_vel[i];
...@@ -647,7 +653,7 @@ void MMMPlayer::run() ...@@ -647,7 +653,7 @@ void MMMPlayer::run()
targetVel = targetVelocities[jointName] = (nextFrame->joint[i] - frame->joint[i]) / (nextFrame->timestep - frame->timestep); targetVel = targetVelocities[jointName] = (nextFrame->joint[i] - frame->joint[i]) / (nextFrame->timestep - frame->timestep);
} }
targetVel *= 0.4;
MMM::MotionFramePtr nextNextFrame; MMM::MotionFramePtr nextNextFrame;
if (currentFrame + 2 < (int)motion->getNumFrames()) if (currentFrame + 2 < (int)motion->getNumFrames())
...@@ -655,12 +661,6 @@ void MMMPlayer::run() ...@@ -655,12 +661,6 @@ void MMMPlayer::run()
nextNextFrame = motion->getMotionFrame(currentFrame + 2); nextNextFrame = motion->getMotionFrame(currentFrame + 2);
} }
/*if (nextNextFrame)
{
float nextTargetVel = (nextNextFrame->joint[i] - nextFrame->joint[i]) / (nextNextFrame->timestep - nextFrame->timestep);
targetVel += interpolationValue * (nextTargetVel - targetVel);
} */
targetVel *= desiredFPS / motionFPS * direction; targetVel *= desiredFPS / motionFPS * direction;
// debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp)); // debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
...@@ -670,25 +670,29 @@ void MMMPlayer::run() ...@@ -670,25 +670,29 @@ void MMMPlayer::run()
//} //}
// targetVelocities[jointName] = 0.0; // targetVelocities[jointName] = 0.0;
auto pid = PIDs.find(jointName); auto pid = PIDs.find(jointName);
if (usePIDController && pid != PIDs.end()) if (usePIDController)
{ {
auto cv = pid->second->getControlValue(); if (pid != PIDs.end())
targetVel += cv; {
auto cv = pid->second->getControlValue();
targetVel += cv;
ARMARX_INFO << deactivateSpam(0.5, jointName) << jointName << " ControlValue " << cv; ARMARX_INFO << deactivateSpam(0.5, jointName) << jointName << " ControlValue " << cv;
if (cv > 20) if (cv > 20)
{ {
ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue; ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
} }
// debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp)); // debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
// debugObserver->setDebugDatafield(jointName, "i", new Variant(pid->second->integral * pid->second->Ki)); // debugObserver->setDebugDatafield(jointName, "i", new Variant(pid->second->integral * pid->second->Ki));
// debugObserver->setDebugDatafield(jointName, "d", new Variant(pid->second->derivative * pid->second->Kd)); // debugObserver->setDebugDatafield(jointName, "d", new Variant(pid->second->derivative * pid->second->Kd));
// debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName])); // debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
}
} }
targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel); targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
...@@ -702,21 +706,17 @@ void MMMPlayer::run() ...@@ -702,21 +706,17 @@ void MMMPlayer::run()
tmpTargetValues = targetPositionValues; tmpTargetValues = targetPositionValues;
} }
// ARMARX_INFO << "target: " << targetPositionValues ;
std::stringstream out;
for (auto v : debugTargetValues)
{ {
out << v.first << ": " << v.second->getFloat() << " " << tmpTargetValues[v.first] << "\n" ; ScopedLock lock(jointAnglesMutex);
updatePIDController(latestJointAngles);
} }
// ARMARX_INFO << "debugtarget: " << out.str() ;
debugObserver->setDebugChannel("targetJointAngles", debugTargetValues); debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
kin->setJointVelocities(targetVelocities); kinematicUnit->setJointVelocities(targetVelocities);
if (!usePIDController) if (!usePIDController)
{ {
kin->setJointAngles(tmpTargetValues); kinematicUnit->setJointAngles(tmpTargetValues);
} }
ARMARX_VERBOSE << deactivateSpam(1) << "Playing frame " << currentFrame; ARMARX_VERBOSE << deactivateSpam(1) << "Playing frame " << currentFrame;
...@@ -726,15 +726,12 @@ void MMMPlayer::run() ...@@ -726,15 +726,12 @@ void MMMPlayer::run()
void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::Current&) void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::Current&)
{ {
ScopedRecursiveLock lock(mmmMutex); ScopedLock lock(jointAnglesMutex);
std::stringstream out; latestJointAngles = angles;
}
for (auto v : angles)
{
out << v.first << ": " << v.second << " " << targetPositionValues[v.first] << "\n" ;
}
// ARMARX_INFO << deactivateSpam(0.5) << "jointangles reported: " << out.str(); void MMMPlayer::updatePIDController(const NameValueMap& angles)
{
if (!usePIDController) if (!usePIDController)
{ {
ARMARX_INFO << deactivateSpam() << "jointangles reporting DISABLED"; ARMARX_INFO << deactivateSpam() << "jointangles reporting DISABLED";
...@@ -743,8 +740,13 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C ...@@ -743,8 +740,13 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
for (const auto & joint : angles) for (const auto & joint : angles)
{ {
const std::string& name = joint.first; const std::string& name = joint.first;
if (targetPositionValues.find(name) == targetPositionValues.end())
{
continue;
}
auto it = PIDs.find(name); auto it = PIDs.find(name);
if (it == PIDs.end()) if (it == PIDs.end())
...@@ -755,21 +757,22 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C ...@@ -755,21 +757,22 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
it = PIDs.find(name); it = PIDs.find(name);
} }
ARMARX_INFO << deactivateSpam() << name;
PIDControllerPtr pid = it->second; PIDControllerPtr pid = it->second;
pid->update(joint.second, targetPositionValues[name]); pid->update(joint.second, targetPositionValues[name]);
} }
} }
void MMMPlayer::onDisconnectComponent() void MMMPlayer::onDisconnectComponent()
{ {
ScopedRecursiveLock lock(mmmMutex);
if (task) if (task)
{ {
task->stop(); task->stop();
} }
kin->stop(); kinematicUnit->stop();
} }
......
source/RobotComponents/components/MMMPlayer/MMMPlayer.h
View file @ 1dc19d03
...@@ -136,13 +136,13 @@ namespace armarx ...@@ -136,13 +136,13 @@ namespace armarx
void run(); void run();
void setJointAngles(int frameNumber); void setJointAngles(int frameNumber);
MMM::MotionPtr motion; MMM::MotionPtr motion;
KinematicUnitInterfacePrx kin; KinematicUnitInterfacePrx kinematicUnit;
DebugObserverInterfacePrx debugObserver; DebugObserverInterfacePrx debugObserver;
PeriodicTask<MMMPlayer>::pointer_type task; PeriodicTask<MMMPlayer>::pointer_type task;
StringList jointNames; StringList jointNames;
NameValueMap jointNamesUsed; NameValueMap jointNamesUsed;
std::string motionPath; std::string motionPath;
int usePIDController; bool usePIDController;
int currentFrame; int currentFrame;
double currentMotionTime, frameOffset; double currentMotionTime, frameOffset;
float motionFPS; float motionFPS;
...@@ -161,8 +161,11 @@ namespace armarx ...@@ -161,8 +161,11 @@ namespace armarx
bool pause(); bool pause();
bool stop(); bool stop();
void load(const std::string& filename, const std::string& projects); void load(const std::string& filename, const std::string& projects);
void updatePIDController(const NameValueMap& angles);
RecursiveMutex mmmMutex; RecursiveMutex mmmMutex;
NameValueMap latestJointAngles;
Mutex jointAnglesMutex;
bool paused; bool paused;
public: public:
...@@ -177,8 +180,8 @@ namespace armarx ...@@ -177,8 +180,8 @@ namespace armarx
virtual bool setLoopPlayback(bool loop, const Ice::Current&); virtual bool setLoopPlayback(bool loop, const Ice::Current&);
virtual std::string getMotionPath(const Ice::Current&); virtual std::string getMotionPath(const Ice::Current&);
virtual StringList getJointNames(const Ice::Current&); virtual StringList getJointNames(const Ice::Current&);
virtual int setUsePID(int usePID, const Ice::Current&); virtual int setUsePID(bool usePID, const Ice::Current&);
virtual void setFPS(int FPS, const Ice::Current&); virtual void setFPS(float FPS, const Ice::Current&);
virtual bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&); virtual bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&);
// KinematicUnitListener interface // KinematicUnitListener interface
......
source/RobotComponents/components/ViewSelection/ViewSelection.cpp
View file @ 1dc19d03
...@@ -49,6 +49,7 @@ void ViewSelection::onInitComponent() ...@@ -49,6 +49,7 @@ void ViewSelection::onInitComponent()
drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue(); drawViewDirection = getProperty<bool>("VisualizeViewDirection").getValue();
randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue(); randomNoiseLevel = getProperty<float>("RandomNoiseLevel").getValue();
centralHeadTiltAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
std::string graphFileName; std::string graphFileName;
...@@ -174,17 +175,16 @@ void ViewSelection::process() ...@@ -174,17 +175,16 @@ void ViewSelection::process()
{ {
try try
{ {
float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
auto syncRobot = robotStateComponent->getSynchronizedRobot();
if (manualViewTargets.size() > 0) if (manualViewTargets.size() > 0)
{ {
// look to the manually set target // look to the manually set target
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
FramedPositionPtr target; FramedPositionPtr target;
{ {
ScopedLock lock(manualViewTargetsMutex); ScopedLock lock(manualViewTargetsMutex);
target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, syncRobot->getName()); target = new FramedPosition(FramedPositionPtr::dynamicCast(manualViewTargets.at(0))->toEigen(), manualViewTargets.at(0)->frame, robotPrx->getName());
for (size_t i = 0; i < manualViewTargets.size() - 1; i++) for (size_t i = 0; i < manualViewTargets.size() - 1; i++)
{ {
...@@ -208,24 +208,18 @@ void ViewSelection::process() ...@@ -208,24 +208,18 @@ void ViewSelection::process()
nodeVisitedForObject.at(i) = -1; nodeVisitedForObject.at(i) = -1;
} }
// generate new random noise
if (randomNoiseLevel > 0)
{
setRandomNoise();
}
auto robotPrx = robotStateComponent->getSynchronizedRobot();
// collect localizable objects // collect localizable objects
std::vector<memoryx::ObjectInstancePtr> localizableObjects; std::vector<memoryx::ObjectInstancePtr> localizableObjects;
std::vector<FramedPositionPtr> localizableObjectPositions; std::vector<FramedPositionPtr> localizableObjectPositions;
float maxObjectDistance = getProperty<float>("MaxObjectDistance").getValue();
SharedRobotInterfacePrx robotPrx = robotStateComponent->getSynchronizedRobot();
int numberOfLostObjects = 0; int numberOfLostObjects = 0;
memoryx::EntityIdList objectIds = objectInstancesProxy->getAllEntityIds(); memoryx::EntityBaseList objectInstances = objectInstancesProxy->getAllEntities();
for (size_t i = 0; i < objectIds.size(); i++) for (size_t i = 0; i < objectInstances.size(); i++)
{ {
const std::string objectId = objectIds.at(i); const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(objectInstances.at(i));
const memoryx::EntityBasePtr entityBase = objectInstancesProxy->getEntityById(objectId);
const memoryx::ObjectInstancePtr object = memoryx::ObjectInstancePtr::dynamicCast(entityBase);
if (object) if (object)
{ {
...@@ -256,6 +250,53 @@ void ViewSelection::process() ...@@ -256,6 +250,53 @@ void ViewSelection::process()
} }
} }
// generate new random noise
if (randomNoiseLevel > 0)
{
// if there are requested objects that were not localized yet, the random noise component will be bigger
memoryx::EntityBaseList requestedObjectClasses = objectClassesProxy->getAllEntities();
bool unlocalizedObjectExists = false;
for (size_t i = 0; i < requestedObjectClasses.size(); i++)
{
bool isInInstancesList = false;
for (size_t j = 0; j < objectInstances.size(); j++)
{
if (requestedObjectClasses.at(i)->getName() == objectInstances.at(j)->getName())
{
isInInstancesList = true;
break;
}
}
if (!isInInstancesList)
{
unlocalizedObjectExists = true;
break;
}
}
if (unlocalizedObjectExists)
{
if (localizableObjects.size() == 0)
{
ARMARX_DEBUG << "There are objects requested, and none of them has been localized yet";
setRandomNoise(centralHeadTiltAngle + 30, 3.0f);
}
else
{
ARMARX_DEBUG << "There are objects requested, and some but not all of them have been localized already";
setRandomNoise(centralHeadTiltAngle + 20, 2.0f);
}
}
else
{
ARMARX_DEBUG << "There are no requested objects that were not localized yet. requestedObjectClasses.size() = " << requestedObjectClasses.size()
<< ", objectInstances.size() = " << objectInstances.size();// << ", difference.size() = " << difference.size();
setRandomNoise(centralHeadTiltAngle, 1.0f);
}
}
const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0; const float probabilityToAddALostObject = (numberOfLostObjects > 0) ? getProperty<float>("ProbabilityToLookForALostObject").getValue() / numberOfLostObjects : 0;
...@@ -279,10 +320,20 @@ void ViewSelection::process() ...@@ -279,10 +320,20 @@ void ViewSelection::process()
if (saliency > 0) if (saliency > 0)
{ {
// close objects get a smaller visibility angle to avoid problems due to the head being positioned unfortunately
float modifiedHalfCameraOpeningAngle = halfCameraOpeningAngle;
float objDist = position->toEigen().norm();
float shortDistance = 0.5f * maxObjectDistance;
if (objDist < shortDistance)
{
modifiedHalfCameraOpeningAngle = (objDist - 0.1f * shortDistance) / (0.9f * shortDistance) * halfCameraOpeningAngle;
modifiedHalfCameraOpeningAngle = std::max(0.0f, modifiedHalfCameraOpeningAngle);
}
TSphereCoord positionInSphereCoordinates; TSphereCoord positionInSphereCoordinates;
MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates); MathTools::convert(position->toEigen() - offsetToHeadCenter, positionInSphereCoordinates);
int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates); int closestNodeIndex = graphLookupTable->getClosestNode(positionInSphereCoordinates);
addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i); addSaliencyRecursive(closestNodeIndex, saliency, positionInSphereCoordinates, i, modifiedHalfCameraOpeningAngle);
} }
} }
} }
...@@ -311,7 +362,7 @@ void ViewSelection::process() ...@@ -311,7 +362,7 @@ void ViewSelection::process()
ARMARX_DEBUG << "Highest saliency: " << maxSaliency; ARMARX_DEBUG << "Highest saliency: " << maxSaliency;
// select a new view if saliency is bigger than threshold, which converges towards 0 over time // select a new view if saliency is bigger than threshold (which converges towards 0 over time)
int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds(); int timeSinceLastViewChange = (IceUtil::Time::now() - timeOfLastViewChange).toMilliSeconds();
float saliencyThreshold = 0; float saliencyThreshold = 0;
...@@ -331,8 +382,11 @@ void ViewSelection::process() ...@@ -331,8 +382,11 @@ void ViewSelection::process()
if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection) if (drawer && robotPrx->hasRobotNode("Cameras") && drawViewDirection)
{ {
Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen()); Vector3Ptr startPos = new Vector3(FramedPosePtr::dynamicCast(robotPrx->getRobotNode("Cameras")->getGlobalPose())->toEigen());
FramedDirectionPtr direction = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName()); FramedDirectionPtr currentDirection = new FramedDirection(Eigen::Vector3f(1, 0, 0), "Cameras", robotPrx->getName());
drawer->setArrowDebugLayerVisu("HeadRealViewDirection", startPos, direction->toGlobal(robotPrx), DrawColor {0, 1, 0, 0.2}, maxObjectDistance, 5); drawer->setArrowDebugLayerVisu("CurrentHeadViewDirection", startPos, currentDirection->toGlobal(robotPrx), DrawColor {0, 0.5, 0.5, 0.1}, maxObjectDistance, 5);
Eigen::Vector3f plannedDirectionEigen = viewTargetPositionPtr->toGlobalEigen(robotPrx) - startPos->toEigen();
Vector3Ptr plannedDirection = new Vector3(plannedDirectionEigen);
drawer->setArrowDebugLayerVisu("PlannedHeadViewDirection", startPos, plannedDirection, DrawColor {0.5, 0.5, 0, 0.1}, maxObjectDistance, 5);
} }
headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr); headIKUnitProxy->setHeadTarget(headIKKinematicChainName, viewTargetPositionPtr);
...@@ -341,6 +395,10 @@ void ViewSelection::process() ...@@ -341,6 +395,10 @@ void ViewSelection::process()
boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges)); boost::this_thread::sleep(boost::posix_time::milliseconds(sleepingTimeBetweenViewDirectionChanges));
} }
} }
else
{
boost::this_thread::sleep(boost::posix_time::milliseconds(5));
}
} }
catch (...) catch (...)
{ {
...@@ -351,11 +409,11 @@ void ViewSelection::process() ...@@ -351,11 +409,11 @@ void ViewSelection::process()
void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex) void ViewSelection::addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc)
{ {
// distance on arc between object projection center and node, // distance on arc between object projection center and node,
// normalized by the maximal viewing angle of the camera (=> in [0,1]) // normalized by the maximal viewing angle of the camera (=> in [0,1])
float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / halfCameraOpeningAngle; float normalizedDistance = MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(currentNodeIndex)->getPosition()) / maxDistanceOnArc;
// increase value of node // increase value of node
float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity() float newValue = ((CIntensityNode*)saliencyEgosphereGraph->getNodes()->at(currentNodeIndex))->getIntensity()
...@@ -374,9 +432,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T ...@@ -374,9 +432,9 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
if (nodeVisitedForObject.at(neighbourIndex) != objectIndex) if (nodeVisitedForObject.at(neighbourIndex) != objectIndex)
{ {
if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= halfCameraOpeningAngle) if (MathTools::getDistanceOnArc(objectSphereCoord, saliencyEgosphereGraph->getNodes()->at(neighbourIndex)->getPosition()) <= maxDistanceOnArc)
{ {
addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex); addSaliencyRecursive(neighbourIndex, saliency, objectSphereCoord, objectIndex, maxDistanceOnArc);
} }
else else
{ {
...@@ -388,7 +446,7 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T ...@@ -388,7 +446,7 @@ void ViewSelection::addSaliencyRecursive(int currentNodeIndex, float saliency, T
void ViewSelection::setRandomNoise() void ViewSelection::setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor)
{ {
if (!robotStateComponent) if (!robotStateComponent)
{ {
...@@ -402,20 +460,21 @@ void ViewSelection::setRandomNoise() ...@@ -402,20 +460,21 @@ void ViewSelection::setRandomNoise()
TSphereCoord currentViewDirection; TSphereCoord currentViewDirection;
MathTools::convert(currentViewTargetEigen, currentViewDirection); MathTools::convert(currentViewTargetEigen, currentViewDirection);
const float centralAngle = getProperty<float>("CentralHeadTiltAngle").getValue();
TNodeList* nodes = randomNoiseGraph->getNodes(); TNodeList* nodes = randomNoiseGraph->getNodes();
const int randomFactor = 100000; const int randomFactor = 100000;
const float noiseFactor = randomNoiseLevel / randomFactor; const float noiseFactor = randomNoiseLevel / randomFactor;
const float distanceWeight = std::min(0.6f / directionVariabilityFactor, 1.0f);
const float centralityWeight = std::min(0.1f / directionVariabilityFactor, 0.2f);
for (size_t i = 0; i < nodes->size(); i++) for (size_t i = 0; i < nodes->size(); i++)
{ {
CIntensityNode* node = (CIntensityNode*) nodes->at(i); CIntensityNode* node = (CIntensityNode*) nodes->at(i);
TSphereCoord nodeCoord = node->getPosition(); TSphereCoord nodeCoord = node->getPosition();
float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord); float distanceOnSphere = MathTools::getDistanceOnArc(currentViewDirection, nodeCoord);
float distanceFactor = 1.0f - 0.5f * distanceOnSphere / M_PI; // prefer directions close to current direction float distanceFactor = 1.0f - distanceWeight * distanceOnSphere / M_PI; // prefer directions close to current direction
float heightFactor = 1.0f - 0.2f * fabs(nodeCoord.fPhi - centralAngle) / 90.0f; // prefer directions that look down 20° float verticalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fPhi - centralAngleForVerticalDirection) / 90.0f; // prefer directions that look to the center
node->setIntensity(heightFactor * distanceFactor * noiseFactor * (rand() % randomFactor)); float horizontalCenterFactor = 1.0f - centralityWeight * fabs(nodeCoord.fTheta) / 120.0f;
node->setIntensity(verticalCenterFactor * horizontalCenterFactor * distanceFactor * noiseFactor * (rand() % randomFactor));
} }
} }
......
source/RobotComponents/components/ViewSelection/ViewSelection.h
View file @ 1dc19d03
...@@ -71,11 +71,11 @@ namespace armarx ...@@ -71,11 +71,11 @@ namespace armarx
defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model"); defineOptionalProperty<std::string>("CameraFrameName", "VirtualCentralGaze", "Name of the frame of the head base in the robot model");
defineOptionalProperty<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation"); defineOptionalProperty<float>("MaxObjectDistance", 1500.f, "Maximum distance of objects the head to be considered for observation");
defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras"); defineOptionalProperty<float>("HalfCameraOpeningAngle", 12.0 * M_PI / 180.0, "0.5 * minimal opening angles of the used cameras");
defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2000, "Time between two view changes, to keep the head looking into one direction for a while (in ms)"); defineOptionalProperty<int>("SleepingTimeBetweenViewDirectionChanges", 2500, "Time between two view changes, to keep the head looking into one direction for a while (in ms)");
defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)"); defineOptionalProperty<bool>("ActiveAtStartup", true, "Decide whether the automatic view selection will be activated (can be changed via the proxy during runtime)");
defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer."); defineOptionalProperty<bool>("VisualizeViewDirection", false, "Draw view ray on DebugLayer.");
defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 50.0f, "Maximal angle the head and eyes can look down (in degrees)"); defineOptionalProperty<float>("MaxOverallHeadTiltAngle", 55.0f, "Maximal angle the head and eyes can look down (in degrees)");
defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees)"); defineOptionalProperty<float>("CentralHeadTiltAngle", 110.0f, "Defines the height direction that will be considered 'central' in the reachable area of the head (in degrees). Default is looking 20 degrees downwards");
defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection"); defineOptionalProperty<float>("ProbabilityToLookForALostObject", 0.03f, "Probability that one of the objects that have been seen but could later not been localized again will be included in the view selection");
defineOptionalProperty<float>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities"); defineOptionalProperty<float>("RandomNoiseLevel", 1.0f, "Maximum for the random noise that will be added to the localization necessities");
...@@ -91,7 +91,7 @@ namespace armarx ...@@ -91,7 +91,7 @@ namespace armarx
* @defgroup Component-ViewSelection ViewSelection * @defgroup Component-ViewSelection ViewSelection
* @ingroup RobotComponents-Components * @ingroup RobotComponents-Components
* @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty * @brief The ViewSelection component controls the head of the robot with inverse kinematics based on the uncertainty
* of the current requested robot locations. * of the current requested object locations.
* The uncertainty of objects grow based on their motion model and the timed passed since the last localization. * The uncertainty of objects grow based on their motion model and the timed passed since the last localization.
* It can be activated or deactivated with the Ice interface and given manual target positions to look at. * It can be activated or deactivated with the Ice interface and given manual target positions to look at.
*/ */
...@@ -143,19 +143,21 @@ namespace armarx ...@@ -143,19 +143,21 @@ namespace armarx
virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) virtual void activateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
{ {
ARMARX_INFO << "activating automatic view selection";
doAutomaticViewSelection = true; doAutomaticViewSelection = true;
} }
virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current()) virtual void deactivateAutomaticViewSelection(const Ice::Current& c = Ice::Current())
{ {
ARMARX_INFO << "DEactivating automatic view selection";
doAutomaticViewSelection = false; doAutomaticViewSelection = false;
} }
private: private:
void process(); void process();
void addSaliencyRecursive(int currentNodeIndex, float saliency, TSphereCoord objectSphereCoord, int objectIndex); void addSaliencyRecursive(const int currentNodeIndex, const float saliency, const TSphereCoord objectSphereCoord, const int objectIndex, const float maxDistanceOnArc);
void setRandomNoise(); void setRandomNoise(const float centralAngleForVerticalDirection, const float directionVariabilityFactor = 1.0f);
RobotStateComponentInterfacePrx robotStateComponent; RobotStateComponentInterfacePrx robotStateComponent;
memoryx::WorkingMemoryInterfacePrx memoryProxy; memoryx::WorkingMemoryInterfacePrx memoryProxy;
...@@ -169,6 +171,7 @@ namespace armarx ...@@ -169,6 +171,7 @@ namespace armarx
std::string headFrameName, cameraFrameName; std::string headFrameName, cameraFrameName;
float randomNoiseLevel; float randomNoiseLevel;
float centralHeadTiltAngle;
armarx::PeriodicTask<ViewSelection>::pointer_type processorTask; armarx::PeriodicTask<ViewSelection>::pointer_type processorTask;
......
source/RobotComponents/gui-plugins/MMMPlayerPlugin/MMMPlayerGuiPlugin.cpp
View file @ 1dc19d03
...@@ -71,6 +71,8 @@ void MMMPlayerWidget::onInitComponent() ...@@ -71,6 +71,8 @@ void MMMPlayerWidget::onInitComponent()
{ {
usingProxy(MMMPLAYER_DEFAULT_NAME); usingProxy(MMMPLAYER_DEFAULT_NAME);
// ui.frameSlider->setEnabled(false); // ui.frameSlider->setEnabled(false);
connectSlots();
} }
...@@ -87,8 +89,6 @@ void MMMPlayerWidget::onConnectComponent() ...@@ -87,8 +89,6 @@ void MMMPlayerWidget::onConnectComponent()
ui.frameSlider->blockSignals(true); ui.frameSlider->blockSignals(true);
ui.frameSlider->setMaximum(MMMPlayer->getNumberOfFrames()); ui.frameSlider->setMaximum(MMMPlayer->getNumberOfFrames());
on_loopPlayback_toggled(false); on_loopPlayback_toggled(false);
connectSlots();
} }
void MMMPlayerWidget::onExitComponent() void MMMPlayerWidget::onExitComponent()
......
source/RobotComponents/gui-plugins/RobotIKPlugin/RobotIKGuiPlugin.cpp
View file @ 1dc19d03
...@@ -305,7 +305,7 @@ armarx::StringList armarx::RobotIKWidgetController::getIncludePaths() ...@@ -305,7 +305,7 @@ armarx::StringList armarx::RobotIKWidgetController::getIncludePaths()
StringList packages = robotStateComponentPrx->getArmarXPackages(); StringList packages = robotStateComponentPrx->getArmarXPackages();
packages.push_back(Application::GetProjectName()); packages.push_back(Application::GetProjectName());
for (const std::string & projectName : packages) for (const std::string& projectName : packages)
{ {
if (projectName.empty()) if (projectName.empty())
{ {
...@@ -416,16 +416,18 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSenso ...@@ -416,16 +416,18 @@ void armarx::RobotIKWidgetController::textFieldUpdateTimerCB(void* data, SoSenso
if (controller->visualization->getIsVisualizing()) if (controller->visualization->getIsVisualizing())
{ {
Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getGlobalPose(); Eigen::Matrix4f tcpMatrix = controller->robot->getRobotNodeSet(controller->ui.comboBox->currentText().toStdString())->getTCP()->getPoseInRootFrame();
FramedPose actualPose(tcpMatrix,
controller->robot->getRootNode()->getName(),
controller->robot->getName());
//Set text label to tcp matrix //Set text label to tcp matrix
std::stringstream buffer; controller->ui.currentPoseMatrix->setText(QString::fromStdString(actualPose.output()));
buffer << tcpMatrix;
std::string matrixText = buffer.str();
controller->ui.currentPoseMatrix->setText(QString::fromStdString(matrixText));
//Set text label for desired tcp pose //Set text label for desired tcp pose
controller->ui.desiredPoseMatrix->setText(QString::fromStdString(controller->visualization->getUserDesiredPoseString())); FramedPose desired(controller->robot->getRootNode()->toLocalCoordinateSystem(controller->visualization->getUserDesiredPose()),
controller->robot->getRootNode()->getName(),
controller->robot->getName());
controller->ui.desiredPoseMatrix->setText(QString::fromStdString(desired.output()));
} }
} }
......
source/RobotComponents/interface/components/DMPComponentBase.ice
View file @ 1dc19d03
...@@ -61,6 +61,7 @@ module armarx ...@@ -61,6 +61,7 @@ module armarx
void trainDMP() throws InvalidArgumentException; void trainDMP() throws InvalidArgumentException;
void storeDMPInDatabase(string cname) throws InvalidArgumentException; void storeDMPInDatabase(string cname) throws InvalidArgumentException;
void readTrajectoryFromFile(string file); void readTrajectoryFromFile(string file);
void setDimensionsToLearn(DVector dimensions);
void removeDMPFromDatabase(string dmpname); void removeDMPFromDatabase(string dmpname);
}; };
......
source/RobotComponents/interface/components/MMMPlayerInterface.ice
View file @ 1dc19d03
...@@ -40,8 +40,8 @@ module armarx ...@@ -40,8 +40,8 @@ module armarx
StringList getJointNames(); StringList getJointNames();
void setCurrentFrame(int frame); void setCurrentFrame(int frame);
bool setLoopPlayback(bool loop); bool setLoopPlayback(bool loop);
int setUsePID(int usePID); int setUsePID(bool usePID);
void setFPS(int FPS); void setFPS(float FPS);
bool setJointsInUse(string jointName, bool inUse); bool setJointsInUse(string jointName, bool inUse);
}; };
}; };
......