VisualServoTowardsTargetPoseWrapper.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::GraspObjectGroup
19  * @author David ( david dot schiebener at kit dot edu )
20  * @date 2014
21  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
22  * GNU General Public License
23  */
24 
26 
27 #include "VisualServoGroupStatechartContext.generated.h"
28 
29 
30 using namespace armarx;
31 using namespace VisualServoGroup;
32 
33 
34 // DO NOT EDIT NEXT LINE
35 VisualServoTowardsTargetPoseWrapper::SubClassRegistry VisualServoTowardsTargetPoseWrapper::Registry(
38 
40  XMLStateConstructorParams stateData) :
42  VisualServoTowardsTargetPoseWrapperGeneratedBase<VisualServoTowardsTargetPoseWrapper>(stateData)
43 {
44 }
45 
46 void
48 {
49  // install event ObjectPoseUpdated
50  VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
51  // FramedPositionPtr objPos = in.getObjectInstanceChannel()->getDataField("position")->get<FramedPosition>();
52 
53  ChannelRefPtr handMemoryChannel = in.getHandMemoryChannel();
54  FramedPositionPtr handPosFromMemoryX;
55  memoryx::ChannelRefBaseSequence instances =
56  context->getObjectMemoryObserver()->getObjectInstances(handMemoryChannel);
57 
58  if (instances.size() == 0)
59  {
60  ARMARX_WARNING << "No instances of the hand in the memory";
61  }
62  else
63  {
64  ARMARX_VERBOSE << "Getting hand pose from memory";
65 
66  handPosFromMemoryX =
67  ChannelRefPtr::dynamicCast(instances.front())->get<FramedPosition>("position");
68  }
69 
70  if (handPosFromMemoryX &&
71  (handPosFromMemoryX->toGlobal(context->getRobot())->toEigen() -
72  in.getTcpTargetPose()->toGlobal(context->getRobot())->toEigen().block<3, 1>(0, 3))
73  .norm() > in.getNoObjectUpdateDistance())
74  // if (!handPosFromMemoryX || (handPosFromMemoryX->toGlobal(context->getRobot())->toEigen()
75  // - in.getTcpTargetPose()->toGlobal(context->getRobot())->toEigen().block<3, 1>(0, 3)).norm() > in.getNoObjectUpdateDistance())
76  {
77  ChannelRefPtr objectInstanceChannel = in.getObjectInstanceChannel();
78  Literal poseUpdated(objectInstanceChannel->observerName + "." +
79  objectInstanceChannel->channelName + ".position",
80  "updated",
81  {});
82  installCondition<ObjectPoseUpdated>(poseUpdated);
83  }
84 }
85 
86 void
88 {
89  // put your user code for the breaking point here
90  // execution time should be short (<100ms)
91 }
92 
93 void
95 {
96  // put your user code for the exit point here
97  // execution time should be short (<100ms)
98 }
99 
100 // DO NOT EDIT NEXT FUNCTION
101 std::string
103 {
104  return "VisualServoTowardsTargetPoseWrapper";
105 }
106 
107 // DO NOT EDIT NEXT FUNCTION
110 {
112 }
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::onEnter
void onEnter() override
Definition: VisualServoTowardsTargetPoseWrapper.cpp:47
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper
Definition: VisualServoTowardsTargetPoseWrapper.h:35
armarx::XMLStateConstructorParams
Definition: XMLState.h:49
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::CreateInstance
static XMLStateFactoryBasePtr CreateInstance(XMLStateConstructorParams stateData)
Definition: VisualServoTowardsTargetPoseWrapper.cpp:109
VisualServoTowardsTargetPoseWrapper.h
armarx::XMLStateFactoryBasePtr
IceInternal::Handle< XMLStateFactoryBase > XMLStateFactoryBasePtr
Definition: XMLState.h:64
IceInternal::Handle< ChannelRef >
armarx::XMLStateTemplate
Class for legacy to stay compatible with old statecharts.
Definition: XMLState.h:146
armarx::FramedPosition
The FramedPosition class.
Definition: FramedPose.h:157
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::onExit
void onExit() override
Definition: VisualServoTowardsTargetPoseWrapper.cpp:94
armarx::Literal
Definition: Term.h:208
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::Registry
static SubClassRegistry Registry
Definition: VisualServoTowardsTargetPoseWrapper.h:50
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::VisualServoTowardsTargetPoseWrapper
VisualServoTowardsTargetPoseWrapper(XMLStateConstructorParams stateData)
Definition: VisualServoTowardsTargetPoseWrapper.cpp:39
ARMARX_WARNING
#define ARMARX_WARNING
Definition: Logging.h:193
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::GetName
static std::string GetName()
Definition: VisualServoTowardsTargetPoseWrapper.cpp:102
armarx::VisualServoGroup::VisualServoTowardsTargetPoseWrapper::onBreak
void onBreak() override
Definition: VisualServoTowardsTargetPoseWrapper.cpp:87
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27