HandUnit.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 Manfred Kroehnert (Manfred dot Kroehnert at kit dot edu)
20  * @date 2013
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 
26 #include "HandUnit.h"
27 
29 
30 #include <VirtualRobot/VirtualRobot.h>
31 #include <VirtualRobot/EndEffector/EndEffector.h>
32 #include <VirtualRobot/XML/RobotIO.h>
33 
34 #include <vector>
35 
36 using namespace armarx;
37 using namespace VirtualRobot;
38 
39 
41 {
42  std::string endeffectorFile = getProperty<std::string>("RobotFileName").getValue();
43  std::string endeffectorName = getProperty<std::string>("EndeffectorName").getValue();
44 
45  if (!ArmarXDataPath::getAbsolutePath(endeffectorFile, endeffectorFile))
46  {
47  throw UserException("Could not find robot file " + endeffectorFile);
48  }
49 
50 
51  try
52  {
53  robot = VirtualRobot::RobotIO::loadRobot(endeffectorFile, VirtualRobot::RobotIO::eStructure);
54  }
55  catch (VirtualRobot::VirtualRobotException& e)
56  {
57  throw UserException(e.what());
58  }
59 
60  if (endeffectorName == "")
61  {
62  throw UserException("EndeffectorName not defined");
63  }
64 
65  if (!robot->hasEndEffector(endeffectorName))
66  {
67  throw UserException("Robot does not contain an endeffector with name: " + endeffectorName);
68  }
69 
70  eef = robot->getEndEffector(endeffectorName);
71  robotName = robot->getName();
72 
73  if (eef->getTcp())
74  {
75  tcpName = robot->getEndEffector(endeffectorName)->getTcp()->getName();
76  }
77  else
78  {
79  throw UserException("Endeffector without TCP: " + endeffectorName);
80  }
81 
82  // get all joints
83  std::vector<EndEffectorActorPtr> actors;
84  eef->getActors(actors);
85 
86  for (size_t i = 0; i < actors.size(); i++)
87  {
88  EndEffectorActorPtr a = actors[i];
89  std::vector<EndEffectorActor::ActorDefinition> ads;// = a->getDefinition();
90 
91  for (size_t j = 0; j < ads.size(); j++)
92  {
93  EndEffectorActor::ActorDefinition ad = ads[j];
94 
95  if (ad.robotNode)
96  {
97  handJoints[ad.robotNode->getName()] = ad.directionAndSpeed;
98  }
99  }
100  }
101 
102  const std::vector<std::string> endeffectorPreshapes = robot->getEndEffector(endeffectorName)->getPreshapes();
103 
104  shapeNames = new SingleTypeVariantList(VariantType::String);
105  std::vector<std::string>::const_iterator iter = endeffectorPreshapes.begin();
106 
107  while (iter != endeffectorPreshapes.end())
108  {
109  Variant currentPreshape;
110  currentPreshape.setString(*iter);
111  shapeNames->addVariant(currentPreshape);
112  iter++;
113  }
114 
115 
116  // component dependencies
117  listenerChannelName = endeffectorName + "State";
118  offeringTopic(listenerChannelName);
119 
120  this->onInitHandUnit();
121 }
122 
123 
125 {
126  ARMARX_INFO << "setting report topic to " << listenerChannelName << flush;
127  listenerPrx = getTopic<HandUnitListenerPrx>(listenerChannelName);
128 
129  this->onStartHandUnit();
130 }
131 
132 
134 {
135  this->onExitHandUnit();
136 }
137 
138 
139 
140 void HandUnit::setShape(const std::string& shapeName, const Ice::Current& c)
141 {
142 }
143 
144 void HandUnit::setShapeWithObjectInstance(const std::string& shapeName, const std::string& objectInstanceName, const Ice::Current& c)
145 {
146  ARMARX_WARNING << "setShapeWithObjectInstance Function not implemented!";
147 }
148 
149 
150 SingleTypeVariantListBasePtr HandUnit::getShapeNames(const Ice::Current& c)
151 {
152  return shapeNames;
153 }
154 
155 
157 {
159  getConfigIdentifier()));
160 }
161 
162 
163 NameValueMap HandUnit::getShapeJointValues(const std::string& shapeName, const Ice::Current&)
164 {
165  EndEffectorPtr efp = robot->getEndEffector(getProperty<std::string>("EndeffectorName").getValue());
166  RobotConfigPtr rc = efp->getPreshape(shapeName);
167  return rc->getRobotNodeJointValueMap();
168 }
169 
171 {
172  NameValueMap result;
173 
174  for (auto j : handJoints)
175  {
176  result[j.first] = 0.0f;
177  }
178 
179  return result;
180 }
181 
182 void HandUnit::setObjectGrasped(const std::string& objectName, const Ice::Current&)
183 {
184  ARMARX_INFO << "Object grasped " << objectName << flush;
185  graspedObject = objectName;
186 }
187 
188 void HandUnit::setObjectReleased(const std::string& objectName, const Ice::Current&)
189 {
190  ARMARX_INFO << "Object released " << objectName << flush;
191  graspedObject = "";
192 }
193 
194 
195 
196 std::string armarx::HandUnit::getHandName(const Ice::Current&)
197 {
198  return eef->getName();
199 }
200 
201 void HandUnit::setJointForces(const NameValueMap& targetJointForces, const Ice::Current&)
202 {
203 
204 }
205 
206 void HandUnit::sendJointCommands(const NameCommandMap& targetJointCommands, const Ice::Current&)
207 {
208 
209 }
210 
211 
212 void armarx::HandUnit::setJointAngles(const armarx::NameValueMap& targetJointAngles, const Ice::Current& c)
213 {
214 
215 }
216 
217 std::string HandUnit::describeHandState(const Ice::Current&)
218 {
219  return "not implemented";
220 }
armarx::Variant
The Variant class is described here: Variants.
Definition: Variant.h:224
armarx::HandUnit::onConnectComponent
void onConnectComponent() override
Calls armarx::PlatformUnit::onStartPlatformUnit().
Definition: HandUnit.cpp:124
VirtualRobot
Definition: FramedPose.h:43
armarx::HandUnit::describeHandState
std::string describeHandState(const Ice::Current &) override
Definition: HandUnit.cpp:217
armarx::Variant::setString
void setString(const std::string &s, const Ice::Current &c=Ice::emptyCurrent) override
Sets the Variant's value to s.
Definition: Variant.cpp:340
armarx::HandUnit::setObjectReleased
void setObjectReleased(const std::string &objectName, const Ice::Current &) override
Definition: HandUnit.cpp:188
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:43
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
armarx::HandUnit::getHandName
std::string getHandName(const Ice::Current &=Ice::emptyCurrent) override
Definition: HandUnit.cpp:196
armarx::HandUnit::setObjectGrasped
void setObjectGrasped(const std::string &objectName, const Ice::Current &) override
Definition: HandUnit.cpp:182
armarx::HandUnit::onInitComponent
void onInitComponent() override
Retrieve proxy for publishing State information and call armarx::PlatformUnit::onInitPlatformUnit().
Definition: HandUnit.cpp:40
armarx::HandUnit::setJointForces
void setJointForces(const NameValueMap &targetJointForces, const Ice::Current &) override
Definition: HandUnit.cpp:201
armarx::HandUnit::getShapeNames
SingleTypeVariantListBasePtr getShapeNames(const Ice::Current &c=Ice::emptyCurrent) override
Definition: HandUnit.cpp:150
armarx::ctrlutil::a
double a(double t, double a0, double j)
Definition: CtrlUtil.h:45
armarx::HandUnit::getCurrentJointValues
NameValueMap getCurrentJointValues(const Ice::Current &c=Ice::emptyCurrent) override
Definition: HandUnit.cpp:170
HandUnit.h
armarx::HandUnit::createPropertyDefinitions
PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: HandUnit.cpp:156
armarx::HandUnit::setShape
void setShape(const std::string &shapeName, const Ice::Current &c=Ice::emptyCurrent) override
Send command to the hand to form a specific shape position.
Definition: HandUnit.cpp:140
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
armarx::HandUnit::setShapeWithObjectInstance
void setShapeWithObjectInstance(const std::string &shapeName, const std::string &objectInstanceName, const Ice::Current &c=Ice::emptyCurrent) override
setShapeWithObjectInstance Send command to the hand to form a specific shape position.
Definition: HandUnit.cpp:144
armarx::HandUnit::getShapeJointValues
NameValueMap getShapeJointValues(const std::string &shapeName, const Ice::Current &c=Ice::emptyCurrent) override
Definition: HandUnit.cpp:163
armarx::HandUnitPropertyDefinitions
Defines all necessary properties for armarx::HandUnit.
Definition: HandUnit.h:44
armarx::HandUnit::onExitComponent
void onExitComponent() override
Calls armarx::PlatformUnit::onExitPlatformUnit().
Definition: HandUnit.cpp:133
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:174
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::HandUnit::sendJointCommands
void sendJointCommands(const NameCommandMap &targetJointCommands, const Ice::Current &) override
Definition: HandUnit.cpp:206
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::HandUnit::setJointAngles
void setJointAngles(const NameValueMap &targetJointAngles, const Ice::Current &) override
Definition: HandUnit.cpp:212
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx::VariantType::String
const VariantTypeId String
Definition: Variant.h:920
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
ArmarXDataPath.h
armarx::VariantType::SingleTypeVariantList
const VariantTypeId SingleTypeVariantList
Definition: SingleTypeVariantList.h:193
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28