27 #include "VisualServoGroupStatechartContext.generated.h"
31 using namespace VisualServoGroup;
50 VisualServoGroupStatechartContext* context = getContext<VisualServoGroupStatechartContext>();
55 memoryx::ChannelRefBaseSequence instances =
56 context->getObjectMemoryObserver()->getObjectInstances(handMemoryChannel);
58 if (instances.size() == 0)
67 ChannelRefPtr::dynamicCast(instances.front())->get<
FramedPosition>(
"position");
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())
77 ChannelRefPtr objectInstanceChannel = in.getObjectInstanceChannel();
78 Literal poseUpdated(objectInstanceChannel->observerName +
"." +
79 objectInstanceChannel->channelName +
".position",
82 installCondition<ObjectPoseUpdated>(poseUpdated);
104 return "VisualServoTowardsTargetPoseWrapper";