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