MovePlatformAfterGrasp.cpp
Go to the documentation of this file.
2 
4 #include <RobotAPI/libraries/skills/provider/SkillProxy.h>
5 
7 
8 #include "util/GraspObjectUtil.h"
9 
10 namespace armarx::skills
11 {
12  namespace
13  {
14  grasp_object::arondto::MovePlatformAfterGraspAcceptedType
15  GetDefaultParameterization()
16  {
17  grasp_object::arondto::MovePlatformAfterGraspAcceptedType ret;
18  ret.orientationalAccuracy = 0.01;
19  ret.positionalAccuracy = 25;
20  return ret;
21  }
22  } // namespace
23 
24  SkillDescription MovePlatformAfterGraspSkill::Description = skills::SkillDescription{
25  "MovePlatformAfterGrasp",
26  "Move Platform after agrasp to ease the navigation",
27  {},
29  grasp_object::arondto::MovePlatformAfterGraspAcceptedType::ToAronType(),
30  GetDefaultParameterization().toAron()};
31 
34  armarx::viz::Client& arviz,
36  TwoArmGraspControlSkill(mns, arviz, Description.skillName, context),
37  SimpleSpecializedSkill<ArgType>(Description)
38  {
39  }
40 
42  MovePlatformAfterGraspSkill::main(const SpecializedMainInput& in)
43  {
45  armem::obj::instance::Reader objectReader(mns);
46 
47  robotReader.connect();
48  objectReader.connect();
49 
50  // //////////////////////////////
51  // get robot
52  // //////////////////////////////
53  auto robot =
54  robotReader.getSynchronizedRobot(in.parameters.robotName,
56  VirtualRobot::RobotIO::RobotDescription::eStructure);
57  if (!robot)
58  {
59  ARMARX_ERROR << "Lost robot.";
60  return {TerminatedSkillStatus::Failed, nullptr};
61  }
62 
63  auto targetPose =
65 
66  skills::platform_control::arondto::MovePlatformToPoseAcceptedType params;
67  params.robotName = in.parameters.robotName;
68  params.orientationalAccuracy = in.parameters.orientationalAccuracy;
69  params.positionalAccuracy = in.parameters.positionalAccuracy;
70  params.pose = targetPose.platformGlobalPose;
71 
72  SkillProxy prx({manager,
75  return {
76  prx.executeFullSkill(getSkillId().toString(in.executorName), params.toAron()).status,
77  nullptr};
78  }
79 } // namespace armarx::skills
armarx::skills::mixin::MNSSkillMixin::mns
armem::client::MemoryNameSystem mns
Definition: MNSSkillMixin.h:11
armarx::armem::robot_state::VirtualRobotReader
The VirtualRobotReader class.
Definition: VirtualRobotReader.h:40
armarx::skills::Skill::manager
manager::dti::SkillManagerInterfacePrx manager
Definition: Skill.h:327
armarx::skills::MovePlatformAfterGraspSkill::MovePlatformAfterGraspSkill
MovePlatformAfterGraspSkill(armem::client::MemoryNameSystem &mns, armarx::viz::Client &arviz, TwoArmGraspControlSkillContext &)
Definition: MovePlatformAfterGrasp.cpp:32
armarx::aron::ret
ReaderT::InputType T & ret
Definition: rw.h:21
armarx::skills
This file is part of ArmarX.
Definition: PeriodicUpdateWidget.cpp:11
armarx::core::time::DateTime::Now
static DateTime Now()
Definition: DateTime.cpp:55
MovePlatformAfterGrasp.h
armarx::skills::TwoArmGraspControlSkill::context
TwoArmGraspControlSkillContext & context
Definition: GraspControlSkill.h:84
armarx::skills::MovePlatformToPose::Description
static SkillDescription Description
Definition: MovePlatformToPose.h:66
armarx::skills::MovePlatformAfterGraspSkill::Description
static SkillDescription Description
Definition: MovePlatformAfterGrasp.h:27
GraspObjectUtil.h
armarx::armem::obj::instance::Reader
Definition: ObjectReader.h:40
MovePlatformToPose.h
armarx::skills::TwoArmGraspControlSkillContext::platformControlSkillProvider
std::string platformControlSkillProvider
Definition: GraspControlSkill.h:51
FramedPose.h
armarx::skills::grasp_control::util::GetPlatformOffsetAfterObjectGraspOutput::platformGlobalPose
Eigen::Matrix4f platformGlobalPose
Definition: GraspObjectUtil.h:66
armarx::skills::SimpleSpecializedSkill
Definition: SimpleSpecializedSkill.h:10
armarx::skills::grasp_control::util::GetGlobalPlatformPoseAfterObjectGrasp
GetPlatformOffsetAfterObjectGraspOutput GetGlobalPlatformPoseAfterObjectGrasp(const GetPlatformOffsetAfterObjectGraspInput &in)
Definition: GraspObjectUtil.cpp:116
ARMARX_ERROR
#define ARMARX_ERROR
Definition: Logging.h:189
armarx::skills::Skill::MainResult
A result struct for th main method of a skill.
Definition: Skill.h:48
armarx::skills::TerminatedSkillStatus::Failed
@ Failed
armarx::skills::SimpleSpecializedSkill< grasp_object::arondto::MovePlatformAfterGraspAcceptedType >::main
Skill::MainResult main() final
Definition: SimpleSpecializedSkill.h:71
armarx::skills::TwoArmGraspControlSkill
Definition: GraspControlSkill.h:77
armarx::viz::toString
const char * toString(InteractionFeedbackType type)
Definition: Interaction.h:27
armarx::skills::TwoArmGraspControlSkillContext
Definition: GraspControlSkill.h:46
armarx::armem::client::MemoryNameSystem
The memory name system (MNS) client.
Definition: MemoryNameSystem.h:69
armarx::skills::MovePlatformAfterGraspSkill::ArgType
grasp_object::arondto::MovePlatformAfterGraspAcceptedType ArgType
Definition: MovePlatformAfterGrasp.h:19
armarx::viz::Client
Definition: Client.h:109
armarx::skills::Skill::getSkillId
SkillID getSkillId() const
Get the id of the skill.
Definition: Skill.h:74
armarx::core::time::Duration::MilliSeconds
static Duration MilliSeconds(std::int64_t milliSeconds)
Constructs a duration in milliseconds.
Definition: Duration.cpp:55