ForceTorqueUnitSimulation.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2012-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotAPI::units
19  * @author Peter Kaiser (peter dot kaiser at kit dot edu)
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
27 
29 
30 #include <SimoxUtility/algorithm/string/string_tools.h>
31 
32 namespace armarx
33 {
35  {
36  int intervalMs = getProperty<int>("IntervalMs").getValue();
37 
38  sensorNamesList = Split(getProperty<std::string>("SensorNames").getValue(), ",");
39  for (auto& sensor : sensorNamesList)
40  {
41  simox::alg::trim(sensor);
42  }
43 
44  for (std::vector<std::string>::iterator s = sensorNamesList.begin(); s != sensorNamesList.end(); s++)
45  {
46  forces[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
47  torques[*s] = new armarx::FramedDirection(Eigen::Vector3f(0, 0, 0), *s, getProperty<std::string>("AgentName").getValue());
48  }
49 
50  ARMARX_VERBOSE << "Starting ForceTorqueUnitSimulation with " << intervalMs << " ms interval";
51  simulationTask = new PeriodicTask<ForceTorqueUnitSimulation>(this, &ForceTorqueUnitSimulation::simulationFunction, intervalMs, false, "ForceTorqueUnitSimulation", false);
52  }
53 
55  {
56  simulationTask->start();
57  }
58 
59 
61  {
62  simulationTask->stop();
63  }
64 
65 
67  {
68 
69  for (auto& sensor : sensorNamesList)
70  {
71  listenerPrx->reportSensorValues(sensor, forces[sensor], torques[sensor]);
72  }
73 
74  //listenerPrx->reportSensorValues(forces);
75  //listenerPrx->reportSensorValues(torques);
76  }
77 
78  void ForceTorqueUnitSimulation::setOffset(const FramedDirectionBasePtr& forceOffsets, const FramedDirectionBasePtr& torqueOffsets, const Ice::Current& c)
79  {
80  // Ignore
81  }
82 
83  void ForceTorqueUnitSimulation::setToNull(const Ice::Current& c)
84  {
85  // Ignore
86  }
87 
88 
90  {
92  }
93 }
armarx::ForceTorqueUnitSimulation::torques
std::map< std::string, armarx::FramedDirectionPtr > torques
Definition: ForceTorqueUnitSimulation.h:96
armarx::ForceTorqueUnitSimulationPropertyDefinitions
Definition: ForceTorqueUnitSimulation.h:43
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::ForceTorqueUnitSimulation::onInitForceTorqueUnit
void onInitForceTorqueUnit() override
Definition: ForceTorqueUnitSimulation.cpp:34
armarx::ForceTorqueUnitSimulation::onStartForceTorqueUnit
void onStartForceTorqueUnit() override
Definition: ForceTorqueUnitSimulation.cpp:54
armarx::ForceTorqueUnitSimulation::simulationFunction
void simulationFunction()
Definition: ForceTorqueUnitSimulation.cpp:66
armarx::Split
std::vector< std::string > Split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelperTemplates.h:35
ForceTorqueUnitSimulation.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::ForceTorqueUnitSimulation::onExitForceTorqueUnit
void onExitForceTorqueUnit() override
Definition: ForceTorqueUnitSimulation.cpp:60
armarx::ForceTorqueUnitSimulation::forces
std::map< std::string, armarx::FramedDirectionPtr > forces
Definition: ForceTorqueUnitSimulation.h:95
armarx::ForceTorqueUnitSimulation::simulationTask
PeriodicTask< ForceTorqueUnitSimulation >::pointer_type simulationTask
Definition: ForceTorqueUnitSimulation.h:98
StringHelpers.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::ForceTorqueUnit::listenerPrx
ForceTorqueUnitListenerPrx listenerPrx
Definition: ForceTorqueUnit.h:93
armarx::VariantType::FramedDirection
const VariantTypeId FramedDirection
Definition: FramedPose.h:38
armarx::ForceTorqueUnitSimulation::sensorNamesList
Ice::StringSeq sensorNamesList
Definition: ForceTorqueUnitSimulation.h:97
armarx::ForceTorqueUnitSimulation::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: ForceTorqueUnitSimulation.cpp:89
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::PeriodicTask
Definition: ArmarXManager.h:70
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx::ForceTorqueUnitSimulation::setToNull
void setToNull(const Ice::Current &c=Ice::emptyCurrent) override
Definition: ForceTorqueUnitSimulation.cpp:83
armarx::ctrlutil::s
double s(double t, double s0, double v0, double a0, double j)
Definition: CtrlUtil.h:33
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::ForceTorqueUnitSimulation::setOffset
void setOffset(const FramedDirectionBasePtr &forceOffsets, const FramedDirectionBasePtr &torqueOffsets, const Ice::Current &c=Ice::emptyCurrent) override
Definition: ForceTorqueUnitSimulation.cpp:78