CalcVelocities.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2014-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 RobotSkillTemplates::FindAndGraspObjectGroup
19  * @author Valerij Wittenbeck ( valerij dot wittenbeck at student dot 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 #include "CalcVelocities.h"
26 #include <VirtualRobot/IK/DifferentialIK.h>
27 #include <VirtualRobot/LinkedCoordinate.h>
28 
29 using namespace armarx;
30 using namespace FindAndGraspObjectGroup;
31 
32 // DO NOT EDIT NEXT LINE
33 CalcVelocities::SubClassRegistry CalcVelocities::Registry(CalcVelocities::GetName(), &CalcVelocities::CreateInstance);
34 
35 
36 
37 CalcVelocities::CalcVelocities(XMLStateConstructorParams stateData) :
38  XMLStateTemplate < CalcVelocities > (stateData)
39 {
40 }
41 
42 void CalcVelocities::onEnter()
43 {
44  // todo: set this via a configuration file
45  ControlMode controlMode = ePositionControl;
46  ARMARX_DEBUG << "Entering CalcVelocities::onEnter()";
47 
48  // TODOS here:
49  // CRITICAL:
50  // * marker to TCP offset (need name as datafield in object channel)
51  // * use relative Target (Done, but still debugging...)
52  // OPTIONAL:
53  // * forces
54 
55  FindAndGraspObjectContext* context = getContext<FindAndGraspObjectContext>();
56 
57  // retrieve channels
58  //ChannelRefPtr objectChannel = getInput<ChannelRef>("objectChannel"); //not needed anymore
59  ChannelRefPtr markerChannel = getInput<ChannelRef>("markerChannel");
60 
61  VirtualRobot::RobotPtr localRobot = context->localRobot;
62  RemoteRobot::synchronizeLocalClone(localRobot, context->robotStateComponent);
63 
64  // use robot localization
65  //PosePtr globalRobotPose = PosePtr::dynamicCast(context->simulatorProxy->getRobotPose(localRobot->getName()));
66  //localRobot->setGlobalPose(globalRobotPose->toEigen());
67  /*
68  float lo,hi,jv;
69  // SIMULATOR
70  ARMARX_INFO << "------------------ SIM -----------------------" << flush;
71  lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Elbow R");
72  hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Elbow R");
73  jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Elbow R");
74  ARMARX_INFO << "Elbow R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
75  lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Underarm R");
76  hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Underarm R");
77  jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Underarm R");
78  ARMARX_INFO << "Underarm R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
79  lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Wrist 1 R");
80  hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Wrist 1 R");
81  jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Wrist 1 R");
82  ARMARX_INFO << "Wrist 1 R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
83  lo = VSContext->simulatorProxy->getRobotJointLimitLo(localRobot->getName(),"Wrist 2 R");
84  hi = VSContext->simulatorProxy->getRobotJointLimitHi(localRobot->getName(),"Wrist 2 R");
85  jv = VSContext->simulatorProxy->getRobotJointAngle(localRobot->getName(),"Wrist 2 R");
86  ARMARX_INFO << "Wrist 2 R (SIM): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
87 
88  PosePtr rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Elbow R"));
89  Eigen::Vector3f gpPos = rnPose->toEigen().block(0,3,3,1);
90  ARMARX_INFO << "Elbow R (SIM): gp:" << gpPos.transpose() << flush;
91  rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Underarm R"));
92  gpPos = rnPose->toEigen().block(0,3,3,1);
93  ARMARX_INFO << "Underarm R (SIM): gp:" << gpPos.transpose() << flush;
94  rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Wrist 1 R"));
95  gpPos = rnPose->toEigen().block(0,3,3,1);
96  ARMARX_INFO << "Wrist 1 R (SIM): gp:" << gpPos.transpose() << flush;
97  rnPose = PosePtr::dynamicCast( VSContext->simulatorProxy->getRobotNodePose(localRobot->getName(),"Wrist 2 R"));
98  gpPos = rnPose->toEigen().block(0,3,3,1);
99  ARMARX_INFO << "Wrist 2 R (SIM): gp:" << gpPos.transpose() << flush;
100 
101 
102  // REMOTE ROBOT
103  ARMARX_INFO << "------------------ REMOTE ROB -----------------------" << flush;
104  armarx::SharedRobotNodeInterfacePrx rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Elbow R");
105  lo = rn->getJointLimitLow();
106  hi = rn->getJointLimitHigh();
107  jv = rn->getJointValue();
108  ARMARX_INFO << "Elbow R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
109 
110  rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Underarm R");
111  lo = rn->getJointLimitLow();
112  hi = rn->getJointLimitHigh();
113  jv = rn->getJointValue();
114  ARMARX_INFO << "Underarm R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
115 
116  rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Wrist 1 R");
117  lo = rn->getJointLimitLow();
118  hi = rn->getJointLimitHigh();
119  jv = rn->getJointValue();
120  ARMARX_INFO << "Wrist 1 R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
121 
122  rn = VSContext->robotStateComponent->getSynchronizedRobot()->getRobotNode("Wrist 2 R");
123  lo = rn->getJointLimitLow();
124  hi = rn->getJointLimitHigh();
125  jv = rn->getJointValue();
126  ARMARX_INFO << "Wrist 2 R (remote robot): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
127 
128 
129  // CLONE local
130  ARMARX_INFO << "------------------ LOCAL ROB CLONE -----------------------" << flush;
131  lo = localRobot->getRobotNode("Elbow R")->getJointLimitLo();
132  hi = localRobot->getRobotNode("Elbow R")->getJointLimitHi();
133  jv = localRobot->getRobotNode("Elbow R")->getJointValue();
134  ARMARX_INFO << "Elbow R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
135 
136  lo = localRobot->getRobotNode("Underarm R")->getJointLimitLo();
137  hi = localRobot->getRobotNode("Underarm R")->getJointLimitHi();
138  jv = localRobot->getRobotNode("Underarm R")->getJointValue();
139  ARMARX_INFO << "Underarm R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
140 
141  lo = localRobot->getRobotNode("Wrist 1 R")->getJointLimitLo();
142  hi = localRobot->getRobotNode("Wrist 1 R")->getJointLimitHi();
143  jv = localRobot->getRobotNode("Wrist 1 R")->getJointValue();
144  ARMARX_INFO << "Wrist 1 R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
145 
146  lo = localRobot->getRobotNode("Wrist 2 R")->getJointLimitLo();
147  hi = localRobot->getRobotNode("Wrist 2 R")->getJointLimitHi();
148  jv = localRobot->getRobotNode("Wrist 2 R")->getJointValue();
149  ARMARX_INFO << "Wrist 2 R (local): lo:" << lo << ", hi:" << hi << ", jv:" << jv << flush;
150 
151  gpPos = localRobot->getRobotNode("Elbow R")->getGlobalPose().block(0,3,3,1);
152  ARMARX_INFO << "Elbow R (local): gp:" << gpPos.transpose() << flush;
153  gpPos = localRobot->getRobotNode("Underarm R")->getGlobalPose().block(0,3,3,1);
154  ARMARX_INFO << "Underarm R (local): gp:" << gpPos.transpose() << flush;
155  gpPos = localRobot->getRobotNode("Wrist 1 R")->getGlobalPose().block(0,3,3,1);
156  ARMARX_INFO << "Wrist 1 R (local): gp:" << gpPos.transpose() << flush;
157  gpPos = localRobot->getRobotNode("Wrist 2 R")->getGlobalPose().block(0,3,3,1);
158  ARMARX_INFO << "Wrist 2 R (local): gp:" << gpPos.transpose() << flush;
159  */
160 
161  // retrieve Jacobian pseudo inverse for TCP and chain
162  std::string robotNodeSetName = getInput<std::string>("kinematicChain");
163  std::string tcpNodeName = getInput<std::string>("tcpName");
164  ARMARX_DEBUG << "robot node set: " << robotNodeSetName << ", tcp: " << tcpNodeName;
165 
166  VirtualRobot::RobotNodeSetPtr robotNodeSet = localRobot->getRobotNodeSet(robotNodeSetName);
167  VirtualRobot::RobotNodePtr tcpNode = localRobot->getRobotNode(tcpNodeName);
168  //Eigen::Matrix4f tcpGlobalPose = tcpNode->getGlobalPose();
169 
170  // use the current tcp pose from the simulator -> there seems to be a slight error between the kinematic model and the bullet simulation (constraint violations!!)
171  // use RobotHandLocalizerDynamicSimulation
172  PosePtr rnPose = PosePtr::dynamicCast(context->simulatorProxy->getRobotNodePose(localRobot->getName(), tcpNodeName));
173  Eigen::Matrix4f tcpGlobalPose = rnPose->toEigen();
174 
175 
176  //VSContext->simulatorProxy->visualizePose("tcpGlobalPose", new FramedPose(tcpGlobalPose, ""));
177  //VSContext->simulatorProxy->visualizePose("Wrist 2 R", new FramedPose(localRobot->getRobotNode("Wrist 2 R")->getGlobalPose(), ""));
178  //VSContext->simulatorProxy->visualizePose("Wrist 1 R", new FramedPose(localRobot->getRobotNode("Wrist 1 R")->getGlobalPose(), ""));
179  //VSContext->simulatorProxy->visualizePose("Underarm R", new FramedPose(localRobot->getRobotNode("Underarm R")->getGlobalPose(), ""));
180  //VSContext->simulatorProxy->visualizePose("Elbow R", new FramedPose(localRobot->getRobotNode("Elbow R")->getGlobalPose(), ""));
181  //VSContext->simulatorProxy->visualizePose("Shoulder 1 R", new FramedPose(localRobot->getRobotNode("Shoulder 1 R")->getGlobalPose(), ""));
182  /*
183  FramedPositionPtr mPos = markerChannel->get<FramedPosition>("position");
184  FramedOrientationPtr mOrient= markerChannel->get<FramedOrientation>("orientation");
185  Eigen::Matrix4f markerLocalPose = Eigen::Matrix4f::Identity();
186  markerLocalPose.block(0,3,3,1) = mPos->toEigen();
187  markerLocalPose.block(0,0,3,3) = mOrient->toEigen();*/
188 
189  // Eigen::Matrix4f markerGlobalPose = localRobot->getRobotNode(mPos->getFrame())->getGlobalPose() * markerLocalPose;
190  // VSContext->simulatorProxy->visualizePose("markerGlobalPose", new FramedPose(markerGlobalPose, ""));
191 
192  //OLD
193  /*
194  // retrieve pose of object
195  Eigen::Matrix4f objectPose_ref = getObjectPoseInRefFrame(objectChannel, refFrame, VSContext);
196  ARMARX_VERBOSE << "Pose of object in reference frame " << objectPose_ref;
197  */
198 
199  //NEW:
200  //relative target pose, computed in VisualServoObject::onEnter()
201  FramedPosePtr targetFramedPose = getInput<FramedPose>("targetPose");
202  Eigen::Matrix4f targetPose = targetFramedPose->toEigen();
203 
204  Eigen::Matrix4f targetGlobalPose = localRobot->getRobotNode(targetFramedPose->getFrame())->toGlobalCoordinateSystem(targetPose);
205 
206  // used debug drawer!
207  //context->simulatorProxy->visualizePose("targetGlobalPose", new FramedPose(targetGlobalPose, ""));
208  //context->simulatorProxy->visualizePose("tcpGlobalPose", new FramedPose(tcpGlobalPose, ""));
209 
210  // retrieve reference frame
211  //std::string refFrame = getInput<std::string>("referenceFrame");
212  std::string refFrame = targetFramedPose->getFrame();
213  ARMARX_VERBOSE << "Using Reference Frame " << refFrame;
214 
215 
216  // retrieve pose of marker
217  // Eigen::Matrix4f markerPose_ref = getObjectPoseInRefFrame(markerChannel, refFrame, VSContext);
218  // ARMARX_VERBOSE << "Pose of marker in reference frame " << markerPose_ref;
219 
220  Eigen::VectorXf errorCartVec(6);
221  ARMARX_DEBUG << "target: " << targetGlobalPose.block(0, 3, 3, 1).transpose();
222  ARMARX_DEBUG << "tcp: " << tcpGlobalPose.block(0, 3, 3, 1).transpose();
223  errorCartVec.block(0, 0, 3, 1) = targetGlobalPose.block(0, 3, 3, 1) - tcpGlobalPose.block(0, 3, 3, 1);
224  // errorCartVec(2) += 180.0f;
225 
226  Eigen::Matrix4f orientation = targetGlobalPose * tcpGlobalPose.inverse();
227  Eigen::AngleAxis<float> aa(orientation.block<3, 3>(0, 0));
228  errorCartVec.tail(3) = aa.axis() * aa.angle();
229 
230  ARMARX_DEBUG << "Cartesian error (RAW)" << errorCartVec.transpose();
231 
232 
233  float distance = errorCartVec.head(3).norm();
234 
235  ARMARX_DEBUG << "Distance: " << distance << flush;
236 
237  if (distance < 10.0f)
238  {
239  EventPtr reached = new EvTargetReached(EVENTTOALL);
240  sendEvent(reached);
241  }
242 
243 
244  float stepSize = getInput<float>("stepSize");
245  errorCartVec = errorCartVec * stepSize;
246 
247  float IKCutCartErrorMM = getInput<float>("IKCutCartErrorMM");
248 
249  if (errorCartVec.head(3).norm() > IKCutCartErrorMM)
250  {
251  errorCartVec = errorCartVec / errorCartVec.head(3).norm() * IKCutCartErrorMM;
252  }
253 
254 
255  ARMARX_DEBUG << "Cartesian error (SCALED, cut:" << IKCutCartErrorMM << " mm)" << errorCartVec.transpose();
256 
257  bool useOrientation = getInput<bool>("useOrientation");
258 
259  // VirtualRobot::DifferentialIK dIK(robotNodeSet,localRobot->getRobotNode(refFrame));
260  VirtualRobot::DifferentialIK dIK(robotNodeSet);
261 
262  Eigen::VectorXf errorJoint(robotNodeSet->getSize());
263 
264  IceUtil::Time startTime;
265  IceUtil::Time endTime;
266  clock_t startT;
267  clock_t endT;
268 
269  if (useOrientation)
270  {
271  startTime = IceUtil::Time::now();
272  startT = clock();
273  Eigen::MatrixXf Ji = dIK.getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::All);
274  endT = clock();
275  endTime = IceUtil::Time::now();
276  ARMARX_DEBUG << "Jacobian (Orientation) " << Ji;
277 
278  // calculate joint error
279  errorJoint = Ji * errorCartVec;
280  }
281  else
282  {
283  startTime = IceUtil::Time::now();
284  startT = clock();
285 
286  Eigen::MatrixXf Ji = dIK.getPseudoInverseJacobianMatrix(tcpNode, VirtualRobot::IKSolver::Position);
287 
288  endT = clock();
289  endTime = IceUtil::Time::now();
290 
291  ARMARX_DEBUG << "Jacobian (Only Position)" << Ji;
292 
293  // calculate joint error
294  Eigen::VectorXf errorPosition(3);
295  errorPosition(0) = errorCartVec(0);
296  errorPosition(1) = errorCartVec(1);
297  errorPosition(2) = errorCartVec(2);
298 
299  ARMARX_DEBUG << "Position error " << errorPosition;
300 
301  errorJoint = Ji * errorPosition;
302  }
303 
304  IceUtil::Time interval = endTime - startTime;
305 
306  if (interval.toMilliSeconds() > 80.0)
307  {
308  float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
309  ARMARX_WARNING << "Jacobi calculation time:" << interval.toMilliSeconds() << " ms (wall time!). Pure CPU time:" << diffClock << "ms";
310  }
311 
312  float VEL_GAIN = 1.0f;
313  float VEL_MAX_COEFF = 1.0f;//0.02f;
314  ARMARX_DEBUG << "JointVelocities " << errorJoint.transpose();
315  ARMARX_DEBUG << "JointVelocities GAIN" << errorJoint.transpose() * VEL_GAIN;
316  float maxV = fabs(errorJoint.maxCoeff());
317 
318  if (maxV > VEL_MAX_COEFF)
319  {
320  errorJoint = errorJoint / maxV * VEL_MAX_COEFF;
321  ARMARX_DEBUG << "JointVelocities VEL_MAX_COEFF" << errorJoint.transpose() * VEL_GAIN;
322  }
323 
324 
325  // build name value map
326  NameValueMap targetVelocities;
327  NameValueMap targetAngles;
328  NameControlModeMap controlModes;
329  const std::vector< VirtualRobot::RobotNodePtr > nodes = robotNodeSet->getAllRobotNodes();
330 
331  for (size_t i = 0; i < nodes.size(); i++)
332  {
333  VirtualRobot::RobotNodePtr n = nodes[i];
334  targetVelocities[n->getName()] = errorJoint(i) * VEL_GAIN;
335  targetAngles[n->getName()] = n->getJointValue() + errorJoint(i);
336  controlModes[n->getName()] = controlMode;//ePositionVelocityControl;
337  //controlModes[n->getName()] = eVelocityControl;
338  }
339 
340  // execute velocities
341  switch (controlMode)
342  {
343  case ePositionControl:
344  ARMARX_DEBUG << "Target Angles: " << targetAngles;
345  context->kinematicUnitPrx->switchControlMode(controlModes);
346  context->kinematicUnitPrx->setJointAngles(targetAngles);
347  break;
348 
349  case eVelocityControl:
350  context->kinematicUnitPrx->switchControlMode(controlModes);
351  context->kinematicUnitPrx->setJointVelocities(targetVelocities);
352  break;
353 
354  case ePositionVelocityControl:
355  context->kinematicUnitPrx->switchControlMode(controlModes);
356  context->kinematicUnitPrx->setJointAngles(targetAngles);
357  context->kinematicUnitPrx->setJointVelocities(targetVelocities);
358  break;
359 
360  default:
361  ARMARX_ERROR << "control mode nyi..." << flush;
362  break;
363  }
364 
365 
366  //VSContext->kinematicUnitPrx->setJoints(targetAngles, targetVelocities);
367 
368  // for now we directly use the simulator to actuate the joints, should be done via the KinematicUnitDynamicsSimulation interface (must implement pos+vel control)!
369  //VSContext->simulatorProxy->actuateRobotJoints(localRobot->getName(), targetAngles, targetVelocities);
370  sendEvent<EvCalcVelocitiesDone>();
371  ARMARX_VERBOSE << "Done CalcVelocities::onEnter()";
372 }
373 
374 void CalcVelocities::onExit()
375 {
376 }
377 
378 Eigen::Matrix4f getObjectPoseInRefFrame(ChannelRefPtr channel, const std::string& refFrame, FindAndGraspObjectContext* context)
379 {
380  FramedPositionPtr position = channel->get<FramedPosition>("position");
381  FramedOrientationPtr orientation = channel->get<FramedOrientation>("orientation");
382 
383  VirtualRobot::LinkedCoordinate markerPose
384  = FramedPose::createLinkedCoordinate(context->remoteRobot, position, orientation);
385 
386  return markerPose.getInFrame(refFrame);
387 }
388 
389 // DO NOT EDIT NEXT FUNCTION
390 std::string CalcVelocities::GetName()
391 {
392  return "CalcVelocities";
393 }
394 
395 // DO NOT EDIT NEXT FUNCTION
396 XMLStateFactoryBasePtr CalcVelocities::CreateInstance(XMLStateConstructorParams stateData)
397 {
398  return XMLStateFactoryBasePtr(new CalcVelocities(stateData));
399 }
400 
armarx::FindAndGraspObjectContext::robotStateComponent
RobotStateComponentInterfacePrx robotStateComponent
Definition: FindAndGraspObjectContext.h:159
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:180
armarx::RemoteRobot::synchronizeLocalClone
static bool synchronizeLocalClone(VirtualRobot::RobotPtr robot, RobotStateComponentInterfacePrx robotStatePrx)
Definition: RemoteRobot.cpp:448
armarx::FramedPose::createLinkedCoordinate
static VirtualRobot::LinkedCoordinate createLinkedCoordinate(const VirtualRobot::RobotPtr &virtualRobot, const FramedPositionPtr &position, const FramedOrientationPtr &orientation)
Definition: FramedPose.cpp:1133
armarx::XMLStateConstructorParams
Definition: XMLState.h:50
armarx::FindAndGraspObjectGroup::CalcVelocities::CalcVelocities
CalcVelocities(XMLStateConstructorParams stateData)
Definition: CalcVelocities.cpp:37
getObjectPoseInRefFrame
Eigen::Matrix4f getObjectPoseInRefFrame(ChannelRefPtr channel, const std::string &refFrame, FindAndGraspObjectContext *context)
Definition: CalcVelocities.cpp:378
armarx::FindAndGraspObjectContext::remoteRobot
VirtualRobot::RobotPtr remoteRobot
Definition: FindAndGraspObjectContext.h:161
armarx::navigation::platform_controller::platform_global_trajectory::NameValueMap
std::map< std::string, float > NameValueMap
Definition: PlatformGlobalTrajectoryController.h:93
armarx::FindAndGraspObjectContext
Definition: FindAndGraspObjectContext.h:67
IceInternal::Handle< ChannelRef >
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:137
armarx::interval
Interval< T > interval(T lo, T hi)
Definition: OccupancyGrid.h:26
CalcVelocities.h
armarx::flush
const LogSender::manipulator flush
Definition: LogSender.h:251
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:177
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:142
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::FramedOrientation
The FramedOrientation class.
Definition: FramedPose.h:199
armarx::FindAndGraspObjectGroup::CalcVelocities::Registry
static SubClassRegistry Registry
Definition: CalcVelocities.h:49
armarx::FindAndGraspObjectGroup::CalcVelocities::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: CalcVelocities.cpp:396
armarx::armem::Time
armarx::core::time::DateTime Time
Definition: forward_declarations.h:13
armarx::FindAndGraspObjectGroup::CalcVelocities::onEnter
void onEnter() override
Virtual function, in which the behaviour of state is defined, when it is entered. Can be overridden,...
Definition: CalcVelocities.cpp:42
armarx::FindAndGraspObjectContext::localRobot
VirtualRobot::RobotPtr localRobot
Definition: FindAndGraspObjectContext.h:171
armarx::FindAndGraspObjectGroup::CalcVelocities::GetName
static std::string GetName()
Definition: CalcVelocities.cpp:390
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:65
armarx::StateUtility::sendEvent
void sendEvent(const EventPtr event, StateBasePtr eventProcessor=nullptr)
Function to send an event to a specific state from an onEnter()-function. Must not be called anywhere...
Definition: StateUtil.cpp:38
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:601
float
#define float
Definition: 16_Level.h:22
armarx::FindAndGraspObjectGroup::CalcVelocities
Definition: CalcVelocities.h:35
distance
double distance(const Point &a, const Point &b)
Definition: point.hpp:88
armarx::FindAndGraspObjectGroup::CalcVelocities::onExit
void onExit() override
Virtual function, in which the behaviour of state is defined, when it is exited. Can be overridden,...
Definition: CalcVelocities.cpp:374
armarx::navigation::core::Position
Eigen::Vector3f Position
Definition: basic_types.h:36
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:186
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::FindAndGraspObjectContext::kinematicUnitPrx
KinematicUnitInterfacePrx kinematicUnitPrx
Definition: FindAndGraspObjectContext.h:160
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:18
EVENTTOALL
#define EVENTTOALL
Definition: StateBase.h:42