PlaceObject.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::PlaceObjectGroup
19  * @author David ( david dot schiebener at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
25 #include "PlaceObject.h"
26 
27 #include <VirtualRobot/Grasping/Grasp.h>
28 #include <VirtualRobot/Grasping/GraspSet.h>
29 
30 #include <MemoryX/core/MemoryXCoreObjectFactories.h>
31 #include <MemoryX/libraries/helpers/VirtualRobotHelpers/SimoxObjectWrapper.h>
32 #include <MemoryX/libraries/memorytypes/MemoryXTypesObjectFactories.h>
33 #include <MemoryX/libraries/memorytypes/entity/ObjectClass.h>
34 #include <MemoryX/libraries/motionmodels/MotionModelStaticObject.h>
35 
36 
37 using namespace armarx;
38 using namespace PlaceObjectGroup;
39 
40 // DO NOT EDIT NEXT LINE
41 PlaceObject::SubClassRegistry PlaceObject::Registry(PlaceObject::GetName(),
42  &PlaceObject::CreateInstance);
43 
44 PlaceObject::PlaceObject(const XMLStateConstructorParams& stateData) :
45  XMLStateTemplate<PlaceObject>(stateData), PlaceObjectGeneratedBase<PlaceObject>(stateData)
46 {
47 }
48 
49 void
50 PlaceObject::onEnter()
51 {
52  const float ZoffsetForPreAndPostPlacePoses = 120;
53  const float xRetreatOffset = in.getXRetreatOffset();
54  const float offsetForRetreatPoseY = 20; //100;
55 
56  PlaceObjectGroupStatechartContextBase* context =
57  getContext<PlaceObjectGroupStatechartContextBase>();
58 
59  std::string handName = in.getHandChannelRef()->getDataField("className")->getString();
60 
61  local.setHandName(handName);
62  //local.setHandShapeOpenName("open");
63 
64  std::string objectName =
65  in.getObjectClassName(); //in.getObjectChannelRef()->getDataField("className")->getString();
66  memoryx::ObjectClassBasePtr objectClassBase =
67  context->getPriorKnowledge()->getObjectClassesSegment()->getObjectClassByName(objectName);
68  Eigen::Matrix3f orientation = Eigen::Matrix3f::Identity();
69  if (in.isPlaceOrientationSet()) // use custom orientation
70  {
71  FramedOrientationPtr ori = in.getPlaceOrientation();
72  orientation =
73  ori->toRootEigen(getRobotStateComponent()->getSynchronizedRobot()).block<3, 3>(0, 0);
74  }
75  Eigen::Matrix4f transformationFromTCPToObject = Eigen::Matrix4f::Identity();
76 
77  if (objectClassBase)
78  {
79  memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(objectClassBase);
80  memoryx::GridFileManagerPtr fileManager(
81  new memoryx::GridFileManager(context->getPriorKnowledge()->getCommonStorage()));
82  objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
83  memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper =
84  objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
85  Eigen::Vector3f placeOrientationAngles = simoxWrapper->getPutdownOrientationRPY();
86 
87  ARMARX_VERBOSE << VAROUT(placeOrientationAngles);
88 
89  /*placeOrientationAngles(0) = -1.5708;
90  placeOrientationAngles(1) = 0;
91  placeOrientationAngles(2) = 0;*/
92  if (!in.isPlaceOrientationSet())
93  {
94  ARMARX_INFO << "Using placing orientation of the object (RPY): "
95  << placeOrientationAngles;
96  orientation = Eigen::AngleAxisf(placeOrientationAngles(0), Eigen::Vector3f::UnitX()) *
97  Eigen::AngleAxisf(placeOrientationAngles(1), Eigen::Vector3f::UnitY()) *
98  Eigen::AngleAxisf(placeOrientationAngles(2), Eigen::Vector3f::UnitZ());
99  }
100  else
101  {
102  ARMARX_INFO << "Using placing orientation from input: " << orientation;
103  }
104 
105  VirtualRobot::GraspSetPtr graspSet =
106  simoxWrapper->getManipulationObject()->getGraspSet(in.getGraspSetName());
107 
108  if (graspSet)
109  {
110  VirtualRobot::GraspPtr grasp = graspSet->getGrasp(in.getGraspName());
111 
112  if (grasp)
113  {
114  transformationFromTCPToObject = grasp->getTransformation();
115  ARMARX_VERBOSE << "Found grasp " << in.getGraspName() << ", trafo:\n"
116  << VAROUT(transformationFromTCPToObject);
117  }
118  else
119  {
120  ARMARX_WARNING << "Grasp " << in.getGraspName() << " not found in grasp set";
121  }
122  }
123  else
124  {
125  ARMARX_WARNING << "Grasp set " << in.getGraspSetName() << " not found";
126  }
127  }
128  else
129  {
130  ARMARX_WARNING << "Object class with name " << objectName
131  << " not found in the classes segment of PriorKnowledge";
132  }
133 
134  Eigen::Matrix4f placePoseObjectLocal = Eigen::Matrix4f::Identity();
135  placePoseObjectLocal.block<3, 3>(0, 0) = orientation;
136  FramedPositionPtr placePoseTemp = in.getPlaceTargetPosition();
137  placePoseTemp->changeFrame(context->getRobot(), context->getRobot()->getRootNode()->getName());
138  placePoseObjectLocal.block<3, 1>(0, 3) = placePoseTemp->toEigen();
139  armarx::FramedPosePtr placeObjectPoseGobal(
140  new FramedPose(placePoseObjectLocal,
141  context->getRobot()->getRootNode()->getName(),
142  context->getRobotStateComponent()->getSynchronizedRobot()->getName()));
143  ARMARX_IMPORTANT << "Placing pose of the object: " << *placeObjectPoseGobal;
144  placeObjectPoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
145  //context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "PlaceTargetObject", placeObjectPoseGobal);
146  getEntityDrawerTopic()->setObjectVisu(
147  "PlaceObject", objectName, objectClassBase, placeObjectPoseGobal);
148  getEntityDrawerTopic()->updateObjectColor("PlaceObject", objectName, DrawColor{0, 0, 0, 0.3});
149 
150  // just for debugging
151  // if (objectClassBase)
152  // {
153  // memoryx::ObjectClassPtr objectClass = memoryx::ObjectClassPtr::dynamicCast(objectClassBase);
154  // memoryx::GridFileManagerPtr fileManager(new memoryx::GridFileManager(context->getPriorKnowledge()->getCommonStorage()));
155  // objectClass->addWrapper(new memoryx::EntityWrappers::SimoxObjectWrapper(fileManager));
156  // memoryx::EntityWrappers::SimoxObjectWrapperPtr simoxWrapper = objectClass->getWrapper<memoryx::EntityWrappers::SimoxObjectWrapper>();
157  // Eigen::Vector3f placeOrientationAnglesVisu = simoxWrapper->getPutdownOrientationRPY();
158  // Eigen::Matrix3f orientationVisu;
159  // orientationVisu = Eigen::AngleAxisf(placeOrientationAnglesVisu(0), Eigen::Vector3f::UnitX())
160  // * Eigen::AngleAxisf(placeOrientationAnglesVisu(1), Eigen::Vector3f::UnitY())
161  // * Eigen::AngleAxisf(placeOrientationAnglesVisu(2), Eigen::Vector3f::UnitZ());
162  // Eigen::Matrix4f placePoseVisu = placeObjectPoseGobal->toEigen();
163  // placePoseVisu.block<3, 3>(0, 0) = orientationVisu;
164  // armarx::FramedPosePtr placePoseVisuFramed(new FramedPose(placePoseVisu, context->getRobot()->getRootNode()->getName(), context->getRobotStateComponent()->getSynchronizedRobot()->getName()));
165  // getEntityDrawerTopic()->setObjectVisu("PlaceObjectVisu", objectName, objectClassBase, placePoseVisuFramed);
166  // getEntityDrawerTopic()->updateObjectColor("PlaceObjectVisu", objectName, DrawColor {0.5, 0.5, 0, 0.3});
167  // // more debugging
168  // VirtualRobot::MathTools::rpy2eigen4f(placeOrientationAnglesVisu(0), placeOrientationAnglesVisu(1), placeOrientationAnglesVisu(2), placePoseVisu);
169  // placePoseVisu.block<3, 1>(0, 3) = placeObjectPoseGobal->toEigen().block<3, 1>(0, 3);
170  // placePoseVisuFramed = new FramedPose(placePoseVisu, context->getRobot()->getRootNode()->getName(), context->getRobotStateComponent()->getSynchronizedRobot()->getName());
171  // getEntityDrawerTopic()->setObjectVisu("PlaceObjectVisu2", objectName, objectClassBase, placePoseVisuFramed);
172  // getEntityDrawerTopic()->updateObjectColor("PlaceObjectVisu2", objectName, DrawColor {0.0, 0.5, 0.5, 0.3});
173  // }
174 
175  Eigen::Matrix4f placePoseTcpLocal =
176  placePoseObjectLocal * transformationFromTCPToObject.inverse();
177  ARMARX_IMPORTANT << "TCP target for putdown: " << placePoseTcpLocal;
178  local.setObjectFinalPose(placeObjectPoseGobal);
179  armarx::FramedPose placePoseTcpFramed(
180  placePoseTcpLocal,
181  context->getRobot()->getRootNode()->getName(),
182  context->getRobotStateComponent()->getSynchronizedRobot()->getName());
183  local.setPlacePose(placePoseTcpFramed);
184  armarx::FramedPosePtr placePoseGobal(new FramedPose(placePoseTcpFramed));
185  placePoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
186  context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "PlaceTargetHand", placePoseGobal);
187 
188  // add z offset
189  placePoseTcpLocal(2, 3) += ZoffsetForPreAndPostPlacePoses;
190  armarx::FramedPose prePlacePose(
191  placePoseTcpLocal,
192  context->getRobot()->getRootNode()->getName(),
193  context->getRobotStateComponent()->getSynchronizedRobot()->getName());
194  local.setPrePlacePose(prePlacePose);
195  armarx::FramedPosePtr prePlacePoseGobal(new FramedPose(prePlacePose));
196  prePlacePoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
197  context->getDebugDrawerTopic()->setPoseVisu(
198  "PlacingPoses", "PrePlaceTarget", prePlacePoseGobal);
199 
200  bool useLeftHand = false;
201 
202  if (handName.find("left") != std::string::npos || handName.find("Left") != std::string::npos)
203  {
204  useLeftHand = true;
205  ARMARX_VERBOSE << "Using left hand";
206  }
207  else
208  {
209  ARMARX_VERBOSE << "Using right hand";
210  }
211 
212  // add x offset
213  if (useLeftHand)
214  {
215  placePoseTcpLocal(0, 3) -= xRetreatOffset;
216  }
217  else
218  {
219  placePoseTcpLocal(0, 3) += xRetreatOffset;
220  }
221 
222  // add y offset
223  placePoseTcpLocal(1, 3) -= offsetForRetreatPoseY;
224 
225  armarx::FramedPose retreatPose(
226  placePoseTcpLocal,
227  context->getRobot()->getRootNode()->getName(),
228  context->getRobotStateComponent()->getSynchronizedRobot()->getName());
229  local.setRetreatPose(retreatPose);
230  armarx::FramedPosePtr retreatPoseGobal(new FramedPose(retreatPose));
231  retreatPoseGobal->changeToGlobal(context->getRobotStateComponent()->getSynchronizedRobot());
232  //context->getDebugDrawerTopic()->setPoseVisu("PlacingPoses", "RetreatTarget", retreatPoseGobal);
233 
234  ARMARX_VERBOSE << "Pre-place pose: " << prePlacePose;
235  ARMARX_VERBOSE << "Place pose: " << placePoseTcpLocal;
236  ARMARX_VERBOSE << "Retreat pose: " << retreatPose;
237 
238  Eigen::Matrix3f ori = placePoseTcpLocal.block<3, 3>(0, 0);
239  Eigen::Vector3f placeOrientationRPY = ori.eulerAngles(0, 1, 2);
240  ARMARX_VERBOSE << "Place orientation angles: " << placeOrientationRPY;
241  ori.transposeInPlace();
242  placeOrientationRPY = ori.eulerAngles(0, 1, 2);
243  ARMARX_VERBOSE << "Place orientation inverse angles: " << placeOrientationRPY;
244 
245 
246  ARMARX_VERBOSE << "Requesting TCP control unit";
247 
248  ARMARX_VERBOSE << "Requested TCP control unit";
249 
250  std::string speechOutput = "I will now put down the " + objectName;
251  ARMARX_INFO << "Speech output: '" << speechOutput << "'";
252  context->getTextToSpeech()->reportText(speechOutput);
253 
254  local.setDecelerationTimeForStopping(1000);
255 }
256 
257 void
258 PlaceObject::onExit()
259 {
260  std::string objectName = in.getObjectClassName();
261  getEntityDrawerTopic()->removeObjectVisu("PlaceObject", objectName);
262  getDebugDrawerTopic()->removeLayer("PlacingPoses");
263 }
264 
265 // DO NOT EDIT NEXT FUNCTION
266 XMLStateFactoryBasePtr
267 PlaceObject::CreateInstance(XMLStateConstructorParams stateData)
268 {
269  return XMLStateFactoryBasePtr(new PlaceObject(stateData));
270 }
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
ARMARX_IMPORTANT
#define ARMARX_IMPORTANT
Definition: Logging.h:190
armarx::PlaceObjectGroup::PlaceObject
Definition: PlaceObject.h:31
armarx::FramedPose
The FramedPose class.
Definition: FramedPose.h:280
armarx::VariantType::FramedPose
const VariantTypeId FramedPose
Definition: FramedPose.h:36
GfxTL::Matrix4f
MatrixXX< 4, 4, float > Matrix4f
Definition: MatrixXX.h:650
armarx::XMLStateConstructorParams
Definition: XMLState.h:49
armarx::PlaceObjectGroup::PlaceObject::onExit
void onExit() override
Definition: PlaceObject.cpp:258
ObjectClass.h
PlaceObject.h
armarx::PlaceObjectGroup::PlaceObject::Registry
static SubClassRegistry Registry
Definition: PlaceObject.h:42
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:64
IceInternal::Handle< FramedOrientation >
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:570
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:146
armarx::PlaceObjectGroup::PlaceObject::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: PlaceObject.cpp:267
MotionModelStaticObject.h
armarx::PlaceObjectGroup::PlaceObject::PlaceObject
PlaceObject(const XMLStateConstructorParams &stateData)
Definition: PlaceObject.cpp:44
MemoryXCoreObjectFactories.h
armarx::PlaceObjectGroup::PlaceObject::onEnter
void onEnter() override
Definition: PlaceObject.cpp:50
memoryx::EntityWrappers::SimoxObjectWrapper
SimoxObjectWrapper offers a simplified access to the Simox ManipulationObject (i.e visualization,...
Definition: SimoxObjectWrapper.h:46
SimoxObjectWrapper.h
ARMARX_INFO
#define ARMARX_INFO
Definition: Logging.h:181
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
VAROUT
#define VAROUT(x)
Definition: StringHelpers.h:198
memoryx::GridFileManager
GridFileManager provides utility functions for working with files in Mongo GridFS and links to them s...
Definition: GridFileManager.h:41
MemoryXTypesObjectFactories.h
GfxTL::Matrix3f
MatrixXX< 3, 3, float > Matrix3f
Definition: MatrixXX.h:649
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