KinematicUnit.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 ArmarXCore::units
19  * @author Christian Boege (boege at kit dot edu)
20  * @date 2011
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #include "KinematicUnit.h"
27 
28 #include <VirtualRobot/VirtualRobot.h>
29 #include <VirtualRobot/XML/RobotIO.h>
30 #include <VirtualRobot/RobotNodeSet.h>
31 #include <VirtualRobot/VirtualRobotException.h>
35 
36 #include <SimoxUtility/algorithm/string/string_tools.h>
37 
38 namespace armarx
39 {
41  {
42  // read names of kinematic chain elements belonging to this unit from XML and setup a map of all joints
43  // the kinematic chain elements belonging to this unit are defined in a RobotNodeSet
44  robotNodeSetName = getProperty<std::string>("RobotNodeSetName").getValue();
45 
46  const std::string project = getProperty<std::string>("RobotFileNameProject").getValue();
47  {
48  std::vector<std::string> result;
49  const auto& packages = armarx::Application::GetProjectDependencies();
50  std::set<std::string> packageSet {packages.begin(), packages.end()};
51  packageSet.emplace(Application::GetProjectName());
52  packageSet.emplace(project);
53  packageSet.erase("");
54  armarXPackages.assign(packageSet.begin(), packageSet.end());
55  }
56 
57  if (relativeRobotFile.empty())
58  {
59  relativeRobotFile = getProperty<std::string>("RobotFileName").getValue();
60  }
61 
62  if (!robot)
63  {
64  Ice::StringSeq includePaths;
65 
66  if (!project.empty())
67  {
69 
70  auto pathsString = finder.getDataDir();
71  Ice::StringSeq projectIncludePaths = simox::alg::split(pathsString, ";,");
72  includePaths.insert(includePaths.end(), projectIncludePaths.begin(), projectIncludePaths.end());
73  }
74  std::string robotFile = relativeRobotFile;
75  if (!ArmarXDataPath::getAbsolutePath(robotFile, robotFile, includePaths))
76  {
77  throw UserException("Could not find robot file " + robotFile);
78  }
79 
80  // read the robot
81  try
82  {
83  robot = VirtualRobot::RobotIO::loadRobot(robotFile, VirtualRobot::RobotIO::eStructure);
84  }
85  catch (VirtualRobot::VirtualRobotException& e)
86  {
87  throw UserException(e.what());
88  }
89  }
90  // read the robot node set and initialize the joints of this kinematic unit
91  if (robotNodeSetName == "")
92  {
93  throw UserException("RobotNodeSet not defined");
94  }
95 
96  VirtualRobot::RobotNodeSetPtr robotNodeSetPtr = robot->getRobotNodeSet(robotNodeSetName);
97  robotNodes = robotNodeSetPtr->getAllRobotNodes();
98 
99  // component dependencies
100  listenerName = getProperty<std::string>("TopicPrefix").getValue() + robotNodeSetName + "State";
102 
103  offeringTopicFromProperty("SkillProviderTopic");
104 
105  this->onInitKinematicUnit();
106  }
107 
108 
110  {
111  ARMARX_INFO << "setting report topic to " << listenerName << flush;
112  listenerPrx = getTopic<KinematicUnitListenerPrx>(listenerName);
113 
114  this->onStartKinematicUnit();
115  }
116 
117 
119  {
120  this->onExitKinematicUnit();
121  }
122 
123 
124  void KinematicUnit::switchControlMode(const NameControlModeMap& targetJointModes, const Ice::Current& c)
125  {
126  }
127 
128  void KinematicUnit::setJointAngles(const NameValueMap& targetJointAngles, const Ice::Current& c)
129  {
130  }
131 
132  void KinematicUnit::setJointVelocities(const NameValueMap& targetJointVelocities, const Ice::Current& c)
133  {
134  }
135 
137  {
138  NameControlModeMap c;
139  NameValueMap t;
140  for (const auto& j : getJoints())
141  {
142  c[j] = ControlMode::ePositionControl;
143  t[j] = 0;
144  }
146  setJointAngles(t);
147  }
149  {
150  NameControlModeMap c;
151  NameValueMap t;
152  for (const auto& j : getJoints())
153  {
154  c[j] = ControlMode::eVelocityControl;
155  t[j] = 0;
156  }
159  }
160 
161  void KinematicUnit::requestKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
162  {
164  }
165 
166  void KinematicUnit::releaseKinematicUnit(const Ice::StringSeq& nodes, const Ice::Current& c)
167  {
169  }
170 
171 
172  std::string KinematicUnit::getRobotFilename(const Ice::Current&) const
173  {
174  return relativeRobotFile;
175  }
176 
177  std::vector<std::string> KinematicUnit::getArmarXPackages(const Ice::Current&) const
178  {
179  return armarXPackages;
180  }
181 
182  std::string KinematicUnit::getRobotName(const Ice::Current&) const
183  {
184  if (robot)
185  {
186  return robot->getName();
187  }
188  else
189  {
190  throw NotInitializedException("Robot Ptr is NULL", "getName");
191  }
192  }
193 
194  std::string KinematicUnit::getRobotNodeSetName(const Ice::Current&) const
195  {
196  if (robotNodeSetName.empty())
197  {
198  throw NotInitializedException("RobotNodeSetName is empty", "getRobotNodeSetName");
199  }
200  return robotNodeSetName;
201  }
202 
203  std::string KinematicUnit::getReportTopicName(const Ice::Current&) const
204  {
205  return listenerName;
206  }
207 
209  {
211  }
212 }
armarx::KinematicUnit::relativeRobotFile
std::string relativeRobotFile
Definition: KinematicUnit.h:143
armarx::Application::GetProjectName
static const std::string & GetProjectName()
Definition: Application.cpp:861
armarx::KinematicUnit::setJointAngles
void setJointAngles(const NameValueMap &targetJointAngles, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnit.cpp:128
armarx::KinematicUnit::onExitComponent
void onExitComponent() override
Hook for subclass.
Definition: KinematicUnit.cpp:118
armarx::SensorActorUnit::release
void release(const Ice::Current &c=Ice::emptyCurrent) override
Release exclusive access to current unit.
Definition: SensorActorUnit.cpp:141
armarx::KinematicUnit::onStartKinematicUnit
virtual void onStartKinematicUnit()=0
armarx::KinematicUnit::robotNodeSetName
std::string robotNodeSetName
Definition: KinematicUnit.h:145
armarx::Component::offeringTopicFromProperty
void offeringTopicFromProperty(const std::string &propertyName)
Offer a topic whose name is specified by the given property.
Definition: Component.cpp:154
armarx::CMakePackageFinder
The CMakePackageFinder class provides an interface to the CMake Package finder capabilities.
Definition: CMakePackageFinder.h:53
armarx::SensorActorUnit::request
void request(const Ice::Current &c=Ice::emptyCurrent) override
Request exclusive access to current unit.
Definition: SensorActorUnit.cpp:123
armarx::KinematicUnit::getArmarXPackages
std::vector< std::string > getArmarXPackages(const Ice::Current &=Ice::emptyCurrent) const override
getArmarXPackages
Definition: KinematicUnit.cpp:177
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::KinematicUnit::setJointAnglesToZero
void setJointAnglesToZero()
Definition: KinematicUnit.cpp:136
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
KinematicUnit.h
project
std::string project
Definition: VisualizationRobot.cpp:83
armarx::KinematicUnit::getReportTopicName
std::string getReportTopicName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: KinematicUnit.cpp:203
armarx::KinematicUnit::getRobotName
std::string getRobotName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: KinematicUnit.cpp:182
armarx::KinematicUnit::onExitKinematicUnit
virtual void onExitKinematicUnit()=0
armarx::KinematicUnit::setJointVelocitiesToZero
void setJointVelocitiesToZero()
Definition: KinematicUnit.cpp:148
armarx::KinematicUnit::armarXPackages
std::vector< std::string > armarXPackages
Definition: KinematicUnit.h:148
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
armarx::KinematicUnit::onConnectComponent
void onConnectComponent() override
Pure virtual hook for the subclass.
Definition: KinematicUnit.cpp:109
armarx::CMakePackageFinder::getDataDir
std::string getDataDir() const
Definition: CMakePackageFinder.h:176
armarx::KinematicUnit::robotNodes
std::vector< VirtualRobot::RobotNodePtr > robotNodes
Definition: KinematicUnit.h:147
armarx::KinematicUnit::setJointVelocities
void setJointVelocities(const NameValueMap &targetJointVelocities, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnit.cpp:132
armarx::KinematicUnitPropertyDefinitions
Definition: KinematicUnit.h:44
armarx::KinematicUnit::getRobotNodeSetName
std::string getRobotNodeSetName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: KinematicUnit.cpp:194
armarx::KinematicUnit::releaseKinematicUnit
virtual void releaseKinematicUnit(const Ice::StringSeq &nodes, const Ice::Current &c=Ice::emptyCurrent)
Definition: KinematicUnit.cpp:166
armarx::KinematicUnit::switchControlMode
void switchControlMode(const NameControlModeMap &targetJointModes, const Ice::Current &c=Ice::emptyCurrent) override
Definition: KinematicUnit.cpp:124
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
CMakePackageFinder.h
armarx::KinematicUnit::listenerName
std::string listenerName
Definition: KinematicUnit.h:146
armarx::KinematicUnit::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: KinematicUnit.cpp:208
armarx::KinematicUnit::onInitComponent
void onInitComponent() override
Pure virtual hook for the subclass.
Definition: KinematicUnit.cpp:40
armarx::KinematicUnit::listenerPrx
KinematicUnitListenerPrx listenerPrx
Definition: KinematicUnit.h:141
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
armarx::ManagedIceObject::offeringTopic
void offeringTopic(const std::string &name)
Registers a topic for retrival after initialization.
Definition: ManagedIceObject.cpp:290
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::KinematicUnit::requestKinematicUnit
virtual void requestKinematicUnit(const Ice::StringSeq &nodes, const Ice::Current &c=Ice::emptyCurrent)
Definition: KinematicUnit.cpp:161
armarx::KinematicUnit::onInitKinematicUnit
virtual void onInitKinematicUnit()=0
armarx::KinematicUnit::getRobotFilename
std::string getRobotFilename(const Ice::Current &=Ice::emptyCurrent) const override
Definition: KinematicUnit.cpp:172
armarx::ArmarXDataPath::getAbsolutePath
static bool getAbsolutePath(const std::string &relativeFilename, std::string &storeAbsoluteFilename, const std::vector< std::string > &additionalSearchPaths={}, bool verbose=true)
Definition: ArmarXDataPath.cpp:111
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::KinematicUnit::robot
VirtualRobot::RobotPtr robot
Definition: KinematicUnit.h:144
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::Application::GetProjectDependencies
static const Ice::StringSeq & GetProjectDependencies()
Definition: Application.cpp:868
Application.h
armarx::split
std::vector< std::string > split(const std::string &source, const std::string &splitBy, bool trimElements=false, bool removeEmptyElements=false)
Definition: StringHelpers.cpp:36