Skip to content
Snippets Groups Projects
Commit 88bcf87e authored by Jonas Beil's avatar Jonas Beil
Browse files

MMMPlayer: Fixed occasional crash

parent 695d798b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotComponents/components/MMMPlayer/MMMPlayer.cpp
+ 66
− 63
View file @ 88bcf87e
......@@ -40,8 +40,8 @@ void MMMPlayer::onInitComponent()
usingProxy(getProperty<std::string>("KinematicUnitName").getValue());
usingProxy("DebugObserver");
motionFPS = 1;
desiredFPS = 1;
motionFPS = 1.0f;
desiredFPS = 1.0f;
frameOffset = 0;
paused = true;
......@@ -50,16 +50,15 @@ void MMMPlayer::onInitComponent()
desiredFPS = getProperty<float>("FPS").getValue();
maxVel = getProperty<float>("absMaximumVelocity").getValue();
if (usePIDController)
{
usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
}
usingTopic(getProperty<std::string>("KinematicTopicName").getValue());
}
void MMMPlayer::onConnectComponent()
{
kin = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
kinematicUnit = getProxy<KinematicUnitInterfacePrx>(getProperty<std::string>("KinematicUnitName").getValue());
debugObserver = getProxy<DebugObserverInterfacePrx>("DebugObserver");
std::string armarxProjects = getProperty<std::string>("ArmarXProjects").getValue();
......@@ -68,7 +67,7 @@ void MMMPlayer::onConnectComponent()
std::vector<std::string> projects;
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::CMakePackageFinder finder(p);
......@@ -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");
}
......@@ -148,8 +147,7 @@ bool MMMPlayer::pauseMMMPlayer(const Ice::Current&)
if (!paused)
{
paused = true;
kin->setJointVelocities(nullVelocities);
sleep(1);
kinematicUnit->setJointVelocities(nullVelocities);
if (direction < 0)
{
......@@ -194,7 +192,7 @@ bool MMMPlayer::stopMMMPlayer(const Ice::Current&)
ARMARX_INFO << "stopped MMMPlayer task from GUI";
}
kin->setJointVelocities(nullVelocities);
kinematicUnit->setJointVelocities(nullVelocities);
return !(task->isRunning());
}
......@@ -224,7 +222,7 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
std::vector<std::string> proj;
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::CMakePackageFinder finder(p);
......@@ -399,8 +397,8 @@ void MMMPlayer::load(const std::string& MMMFile, const std::string& projects)
jointNamesUsed[jointName] = true;
}
kin->switchControlMode(modes);
kin->setJointVelocities(nullVelocities);
kinematicUnit->switchControlMode(modes);
kinematicUnit->setJointVelocities(nullVelocities);
}
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
int MMMPlayer::getNumberOfFrames(const Ice::Current&)
{
if (!motion)
{
return 0;
......@@ -435,6 +434,7 @@ int MMMPlayer::getCurrentFrame(const Ice::Current&)
bool MMMPlayer::setLoopPlayback(bool loop, const Ice::Current&)
{
loopPlayback = loop;
return loopPlayback;
}
......@@ -449,8 +449,11 @@ StringList MMMPlayer::getJointNames(const Ice::Current&)
return jointNames;
}
int MMMPlayer::setUsePID(int usePID, const Ice::Current&)
int MMMPlayer::setUsePID(bool usePID, const Ice::Current&)
{
ScopedRecursiveLock lock(mmmMutex);
usePIDController = usePID;
NameControlModeMap modes;
......@@ -470,14 +473,14 @@ int MMMPlayer::setUsePID(int usePID, const Ice::Current&)
try
{
kin->switchControlMode(modes);
kinematicUnit->switchControlMode(modes);
}
catch (...) { }
return usePIDController;
}
void MMMPlayer::setFPS(int FPS, const Ice::Current&)
void MMMPlayer::setFPS(float FPS, const Ice::Current&)
{
desiredFPS = FPS;
ARMARX_VERBOSE << "set FPS to " << FPS;
......@@ -492,6 +495,9 @@ bool MMMPlayer::setJointsInUse(const std::string& jointName, bool inUse, const I
void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
{
ScopedRecursiveLock lock(mmmMutex);
if (!motion)
{
return;
......@@ -534,15 +540,14 @@ void MMMPlayer::setCurrentFrame(int frameNumber, const Ice::Current&)
}
debugObserver->setDebugChannel("targetJointAngles", debugTargetValues);
kin->setJointVelocities(nullVelocities);
kin->setJointAngles(targetPositionValues);
kinematicUnit->setJointVelocities(nullVelocities);
kinematicUnit->setJointAngles(targetPositionValues);
ARMARX_INFO << "MMMPlayer: Current frame set to " << frameNumber;
}
void MMMPlayer::run()
{
NameValueMap targetVelocities;
if (!motion)
......@@ -574,7 +579,7 @@ void MMMPlayer::run()
{
ARMARX_VERBOSE << "current frame " << currentFrame;
kin->setJointVelocities(nullVelocities);
kinematicUnit->setJointVelocities(nullVelocities);
if (loopPlayback)
{
......@@ -609,6 +614,7 @@ void MMMPlayer::run()
ARMARX_INFO << deactivateSpam(1) << "frame: " << currentFrame << " " << VAROUT(interpolationValue);
ARMARX_CHECK_EXPRESSION((int)jointNames.size() == frame->joint.rows());
for (size_t i = 0; i < jointNames.size(); ++i)
{
const auto& jointName = jointNames.at(i);
......@@ -629,7 +635,7 @@ void MMMPlayer::run()
targetPosValue += it->second;
}
assert(targetPosValue == targetPositionValues[jointName]);
// assert(targetPosValue == targetPositionValues[jointName]);
debugTargetValues[jointName] = new Variant(targetPosValue);
float& targetVel = targetVelocities[jointName] = 0.0;
auto vel = frame->joint_vel[i];
......@@ -647,7 +653,7 @@ void MMMPlayer::run()
targetVel = targetVelocities[jointName] = (nextFrame->joint[i] - frame->joint[i]) / (nextFrame->timestep - frame->timestep);
}
targetVel *= 0.4;
MMM::MotionFramePtr nextNextFrame;
if (currentFrame + 2 < (int)motion->getNumFrames())
......@@ -655,12 +661,6 @@ void MMMPlayer::run()
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;
// debugObserver->setDebugDatafield(jointName, "p", new Variant(pid->second->previousError * pid->second->Kp));
......@@ -670,25 +670,29 @@ void MMMPlayer::run()
//}
// targetVelocities[jointName] = 0.0;
auto pid = PIDs.find(jointName);
if (usePIDController && pid != PIDs.end())
if (usePIDController)
{
auto cv = pid->second->getControlValue();
targetVel += cv;
if (pid != PIDs.end())
{
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)
{
ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
}
if (cv > 20)
{
ARMARX_INFO << "" << (jointName) << ": v:" << (targetVel) << " pos: " << targetPosValue;
}
// 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, "d", new Variant(pid->second->derivative * pid->second->Kd));
// debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
// 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, "d", new Variant(pid->second->derivative * pid->second->Kd));
// debugObserver->setDebugDatafield(jointName, "velocity", new Variant(targetVelocities[jointName]));
}
}
targetVel = std::min<double>(maxVel / 180.0 * M_PI, targetVel);
......@@ -702,21 +706,17 @@ void MMMPlayer::run()
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);
kin->setJointVelocities(targetVelocities);
kinematicUnit->setJointVelocities(targetVelocities);
if (!usePIDController)
{
kin->setJointAngles(tmpTargetValues);
kinematicUnit->setJointAngles(tmpTargetValues);
}
ARMARX_VERBOSE << deactivateSpam(1) << "Playing frame " << currentFrame;
......@@ -726,15 +726,12 @@ void MMMPlayer::run()
void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::Current&)
{
ScopedRecursiveLock lock(mmmMutex);
std::stringstream out;
for (auto v : angles)
{
out << v.first << ": " << v.second << " " << targetPositionValues[v.first] << "\n" ;
}
ScopedLock lock(jointAnglesMutex);
latestJointAngles = angles;
}
// ARMARX_INFO << deactivateSpam(0.5) << "jointangles reported: " << out.str();
void MMMPlayer::updatePIDController(const NameValueMap& angles)
{
if (!usePIDController)
{
ARMARX_INFO << deactivateSpam() << "jointangles reporting DISABLED";
......@@ -743,8 +740,13 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
for (const auto & joint : angles)
{
const std::string& name = joint.first;
if (targetPositionValues.find(name) == targetPositionValues.end())
{
continue;
}
auto it = PIDs.find(name);
if (it == PIDs.end())
......@@ -755,21 +757,22 @@ void MMMPlayer::reportJointAngles(const NameValueMap& angles, bool, const Ice::C
it = PIDs.find(name);
}
ARMARX_INFO << deactivateSpam() << name;
PIDControllerPtr pid = it->second;
pid->update(joint.second, targetPositionValues[name]);
}
}
void MMMPlayer::onDisconnectComponent()
{
ScopedRecursiveLock lock(mmmMutex);
if (task)
{
task->stop();
}
kin->stop();
kinematicUnit->stop();
}
......
source/RobotComponents/components/MMMPlayer/MMMPlayer.h
+ 7
− 4
View file @ 88bcf87e
......@@ -136,13 +136,13 @@ namespace armarx
void run();
void setJointAngles(int frameNumber);
MMM::MotionPtr motion;
KinematicUnitInterfacePrx kin;
KinematicUnitInterfacePrx kinematicUnit;
DebugObserverInterfacePrx debugObserver;
PeriodicTask<MMMPlayer>::pointer_type task;
StringList jointNames;
NameValueMap jointNamesUsed;
std::string motionPath;
int usePIDController;
bool usePIDController;
int currentFrame;
double currentMotionTime, frameOffset;
float motionFPS;
......@@ -161,8 +161,11 @@ namespace armarx
bool pause();
bool stop();
void load(const std::string& filename, const std::string& projects);
void updatePIDController(const NameValueMap& angles);
RecursiveMutex mmmMutex;
NameValueMap latestJointAngles;
Mutex jointAnglesMutex;
bool paused;
public:
......@@ -177,8 +180,8 @@ namespace armarx
virtual bool setLoopPlayback(bool loop, const Ice::Current&);
virtual std::string getMotionPath(const Ice::Current&);
virtual StringList getJointNames(const Ice::Current&);
virtual int setUsePID(int usePID, const Ice::Current&);
virtual void setFPS(int FPS, const Ice::Current&);
virtual int setUsePID(bool usePID, const Ice::Current&);
virtual void setFPS(float FPS, const Ice::Current&);
virtual bool setJointsInUse(const std::string& jointName, bool inUse, const Ice::Current&);
// KinematicUnitListener interface
......
source/RobotComponents/interface/components/MMMPlayerInterface.ice
+ 2
− 2
View file @ 88bcf87e
......@@ -40,8 +40,8 @@ module armarx
StringList getJointNames();
void setCurrentFrame(int frame);
bool setLoopPlayback(bool loop);
int setUsePID(int usePID);
void setFPS(int FPS);
int setUsePID(bool usePID);
void setFPS(float FPS);
bool setJointsInUse(string jointName, bool inUse);
};
};
......
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