KinematicSubUnit.cpp 9.23 KiB
/*
* This file is part of ArmarX.
*
* ArmarX is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 as
* published by the Free Software Foundation.
*
* ArmarX is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*
* @package RobotAPI::ArmarXObjects::KinematicSubUnit
* @author Raphael ( raphael dot grimm at student dot kit dot edu )
* @date 2017
* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
* GNU General Public License
*/
#include "KinematicSubUnit.h"
void armarx::KinematicSubUnit::setupData(std::string relRobFile, VirtualRobot::RobotPtr rob, std::map<std::string, ActuatorData> newDevs, RobotUnit* newRobotUnit)
{
std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_CHECK_EXPRESSION(getState() == eManagedIceObjectCreated);
ARMARX_CHECK_EXPRESSION(!robot);
ARMARX_CHECK_EXPRESSION(rob);
robot = rob;
ARMARX_CHECK_EXPRESSION(!robotUnit);
ARMARX_CHECK_EXPRESSION(newRobotUnit);
robotUnit = newRobotUnit;
ARMARX_CHECK_EXPRESSION(relativeRobotFile.empty());
ARMARX_CHECK_EXPRESSION(!relRobFile.empty());
relativeRobotFile = relRobFile;
devs = std::move(newDevs);
}
void armarx::KinematicSubUnit::update(const armarx::SensorAndControl& sc, const armarx::LVL0AndLVL1Controllers& c)
{
if (!getProxy())
{
//this unit is not initialized yet
ARMARX_IMPORTANT << deactivateSpam(1) << "not initialized yet";
return;
}
if (!listenerPrx)
{
ARMARX_IMPORTANT << deactivateSpam(1) << "listener is not set";
return;
}
std::lock_guard<std::mutex> guard {dataMutex};
bool ctrlModesAValueChanged = false;
bool angAValueChanged = false;
bool velAValueChanged = false;
bool accAValueChanged = false;
bool torAValueChanged = false;
bool motorCurrentAValueChanged = false;
bool motorTemperaturesAValueChanged = false;
long timestamp = sc.sensorValuesTimestamp.toMicroSeconds();
std::set<LVL1Controller*> lvl1s {c.lvl1Controllers.begin(), c.lvl1Controllers.end()};
std::vector<std::string> actuatorsWithoutSensor;
actuatorsWithoutSensor.reserve(devs.size());
for (const auto& name2actuatorData : devs) {
const ActuatorData& actuatorData = name2actuatorData.second;
const auto& name = actuatorData.name;
//mode
{
ControlMode c = eUnknown;
if (actuatorData.ctrlPos && lvl1s.count(actuatorData.ctrlPos.get()))
{
c = ePositionControl;
}
if (actuatorData.ctrlVel && lvl1s.count(actuatorData.ctrlVel.get()))
{
ARMARX_CHECK_EXPRESSION(eUnknown == c);
c = eVelocityControl;
}
if (actuatorData.ctrlTor && lvl1s.count(actuatorData.ctrlTor.get()))
{
ARMARX_CHECK_EXPRESSION(eUnknown == c);
c = eTorqueControl;
}
ctrlModesAValueChanged = ctrlModesAValueChanged || !ctrlModes.count(name) || (ctrlModes.at(name) != c);
ctrlModes[name] = c;
}
if (actuatorData.sensorDeviceIndex < sc.sensors.size())
{
const SensorValueBase* s = sc.sensors.at(actuatorData.sensorDeviceIndex).get();
UpdateNameValueMap<SensorValue1DoFActuatorPosition , &SensorValue1DoFActuatorPosition::position >(ang, name, s, angAValueChanged);
UpdateNameValueMap<SensorValue1DoFActuatorVelocity , &SensorValue1DoFActuatorVelocity::velocity >(vel, name, s, velAValueChanged);
UpdateNameValueMap<SensorValue1DoFActuatorAcceleration , &SensorValue1DoFActuatorAcceleration::acceleration >(acc, name, s, accAValueChanged);
UpdateNameValueMap<SensorValue1DoFActuatorTorque , &SensorValue1DoFActuatorTorque::torque >(tor, name, s, torAValueChanged);
UpdateNameValueMap<SensorValue1DoFActuatorCurrent , &SensorValue1DoFActuatorCurrent::motorCurrent >(motorCurrents, name, s, motorCurrentAValueChanged);
UpdateNameValueMap<SensorValue1DoFActuatorMotorTemperature, &SensorValue1DoFActuatorMotorTemperature::motorTemperature>(motorTemperatures, name, s, motorTemperaturesAValueChanged);
}
else
{
actuatorsWithoutSensor.emplace_back(name);
}
}
if (!actuatorsWithoutSensor.empty())
{
ARMARX_WARNING << deactivateSpam(5) << "these actuators have no sensor!:\n" << actuatorsWithoutSensor;
}
ARMARX_DEBUG << deactivateSpam(30) << "reporting updated data:\n"
<< ctrlModes.size() << " \tcontrol modes (updated = " << ctrlModesAValueChanged << ")\n"
<< ang.size() << " \tjoint angles (updated = " << angAValueChanged << ")\n"
<< vel.size() << " \tjoint velocities (updated = " << velAValueChanged << ")\n"
<< acc.size() << " \tjoint accelerations (updated = " << accAValueChanged << ")\n"
<< tor.size() << " \tjoint torques (updated = " << torAValueChanged << ")\n"
<< motorCurrents.size() << " \tmotor currents (updated = " << motorCurrentAValueChanged << ")\n"
<< motorTemperatures.size() << " \tmotor temperatures (updated = " << motorTemperaturesAValueChanged << ")\n";
auto prx = listenerPrx->ice_batchOneway();
prx->reportControlModeChanged(ctrlModes, timestamp, ctrlModesAValueChanged);
prx->reportJointAngles(ang, timestamp, angAValueChanged);
prx->reportJointVelocities(vel, timestamp, velAValueChanged);
prx->reportJointAccelerations(acc, timestamp, accAValueChanged);
prx->reportJointTorques(tor, timestamp, torAValueChanged);
prx->reportJointCurrents(motorCurrents, timestamp, motorCurrentAValueChanged);
prx->reportJointMotorTemperatures(motorTemperatures, timestamp, motorTemperaturesAValueChanged);
prx->ice_flushBatchRequests();
}
void armarx::KinematicSubUnit::requestJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
void armarx::KinematicSubUnit::releaseJoints(const Ice::StringSeq&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
void armarx::KinematicSubUnit::setJointAngles(const armarx::NameValueMap& angles, const Ice::Current&)
{
std::lock_guard<std::mutex> guard {dataMutex};
for (const auto& n2v : angles)
{
if (devs.count(n2v.first))
{
devs.at(n2v.first).ctrlPos->set(n2v.second);
}
}
}
void armarx::KinematicSubUnit::setJointVelocities(const armarx::NameValueMap& velocities, const Ice::Current&)
{
std::lock_guard<std::mutex> guard {dataMutex};
ARMARX_IMPORTANT << VAROUT(velocities);
for (const auto& n2v : velocities)
{
if (devs.count(n2v.first))
{
devs.at(n2v.first).ctrlVel->set(n2v.second);
}
}
}
void armarx::KinematicSubUnit::setJointTorques(const armarx::NameValueMap& torques, const Ice::Current&)
{
std::lock_guard<std::mutex> guard {dataMutex};
for (const auto& n2v : torques)
{
if (devs.count(n2v.first))
{
devs.at(n2v.first).ctrlTor->set(n2v.second);
}
}
}
void armarx::KinematicSubUnit::switchControlMode(const armarx::NameControlModeMap& ncm, const Ice::Current&)
{
std::vector<std::string> toActivate;
{
std::lock_guard<std::mutex> guard {dataMutex};
toActivate.reserve(ncm.size());
for (const auto& n2c : ncm)
{
if (devs.count(n2c.first))
{
auto ctrl = devs.at(n2c.first).getController(n2c.second);
if (ctrl && !ctrl->isControllerActive())
{
toActivate.emplace_back(ctrl->getInstanceName());
}
}
}
ARMARX_VERBOSE << "switching control modes activates these LVL1Controllers:\n" << toActivate << std::flush;
ARMARX_CHECK_EXPRESSION(robotUnit);
}
robotUnit->activateLVL1Controllers(toActivate);
}
void armarx::KinematicSubUnit::setJointAccelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
void armarx::KinematicSubUnit::setJointDecelerations(const armarx::NameValueMap&, const Ice::Current&)
{
ARMARX_WARNING << "NYI";
}
armarx::NameControlModeMap armarx::KinematicSubUnit::getControlModes(const Ice::Current&)
{
std::lock_guard<std::mutex> guard {dataMutex};
return ctrlModes;
}
const armarx::LVL1ControllerPtr armarx::KinematicSubUnit::ActuatorData::getController(armarx::ControlMode c) const
{
switch (c)
{
case ePositionVelocityControl:
return ctrlPos;
case ePositionControl:
return ctrlPos;
case eVelocityControl:
return ctrlVel;
case eTorqueControl:
return ctrlTor;
default:
return nullptr;
}
}