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