Skip to content
Snippets Groups Projects
Commit 677eaa03 authored by armar-user's avatar armar-user
Browse files

some cleaning and copying the cleaned code to ScanLocation skill

parent ca360831
No related branches found
No related tags found
1 merge request!16Feature/skills
Hide whitespace changes
Inline Side-by-side
source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp
+ 24
− 11
View file @ 677eaa03
...@@ -2,7 +2,8 @@ ...@@ -2,7 +2,8 @@
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h" #include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h" #include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h" #include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
#include <VirtualRobot/Robot.h>
#include <memory> #include <memory>
#include <Eigen/Core> #include <Eigen/Core>
...@@ -15,6 +16,8 @@ namespace armarx::view_selection::skills ...@@ -15,6 +16,8 @@ namespace armarx::view_selection::skills
{ {
std::map<std::string, float> neck_joint_angles = { std::map<std::string, float> neck_joint_angles = {
{"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}}; {"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
VirtualRobot::RobotPtr localRobot;
LookForObjectWithKinematicUnit::LookForObjectWithKinematicUnit() : Base(DefaultSkillDescription()) LookForObjectWithKinematicUnit::LookForObjectWithKinematicUnit() : Base(DefaultSkillDescription())
{ {
} }
...@@ -51,7 +54,6 @@ namespace armarx::view_selection::skills ...@@ -51,7 +54,6 @@ namespace armarx::view_selection::skills
{ {
float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num; float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num; float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
srv_->kinematicUnit->setJointAngles(neck_joint_angles);
for (int i = 1; i < waypoints_num; i++) for (int i = 1; i < waypoints_num; i++)
{ {
neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size; neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
...@@ -59,9 +61,15 @@ namespace armarx::view_selection::skills ...@@ -59,9 +61,15 @@ namespace armarx::view_selection::skills
srv_->kinematicUnit->setJointAngles({ srv_->kinematicUnit->setJointAngles({
{"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}}); {"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
std::this_thread::sleep_for(std::chrono::milliseconds(200)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
// while (localRobot->getRobotNode("Neck_1_Yaw")->getJointValue() != neck_joint_angles["Neck_1_Yaw"]) {
// srv_->kinematicUnit->setJointAngles({
// {"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
// armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
// }
} }
//std::this_thread::sleep_for(std::chrono::milliseconds(200)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10)); //armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
} }
void void
LookForObjectWithKinematicUnit::speak(const std::string& text) LookForObjectWithKinematicUnit::speak(const std::string& text)
...@@ -96,17 +104,20 @@ namespace armarx::view_selection::skills ...@@ -96,17 +104,20 @@ namespace armarx::view_selection::skills
// } // }
// std::optional<objpose::ObjectPose> objectPose = findObjectByClass(objectPoses, objectId); // std::optional<objpose::ObjectPose> objectPose = findObjectByClass(objectPoses, objectId);
armarx::RemoteRobot::synchronizeLocalClone(localRobot, srv_->robotStateComponent);
srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl} });
ARMARX_INFO << "start the running";
// doing the neck movement routine // doing the neck movement routine
// move neck to start position // move neck to start position
setNeckJointAngles(0.0, 0.2, 10); setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "finished first movement'";
// move the neck to the max right // move the neck to the max right
setNeckJointAngles(-0.593412, 0.7609636, 10); setNeckJointAngles(-0.593412, 0.7609636, 10);
// move the neck to the max left // move the neck to the max left
setNeckJointAngles(0.593412, 0.7609636, 20); setNeckJointAngles(0.593412, 0.7609636, 20);
// move the neck back to the normal // move the neck back to the normal
setNeckJointAngles(0.0, 0.2, 10); setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "no problem with the neck";
// getting the // getting the
objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses(); objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
...@@ -129,16 +140,18 @@ namespace armarx::view_selection::skills ...@@ -129,16 +140,18 @@ namespace armarx::view_selection::skills
} }
} }
for (int i = 0; i < objects_names.size(); i++) { ARMARX_INFO << "no problem with the poses";
for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
if (i == 0) { if (i == 0) {
speak("I can see the following objects"); speak("I can see the following objects");
armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
} else if (i == (objects_names.size() - 1)) { } else if (i == (static_cast<int>(objects_names.size()) - 1)) {
speak("and"); speak("and");
armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
} }
speak(objects_names[i]); speak(objects_names[i]);
armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(10)); std::this_thread::sleep_for(std::chrono::milliseconds(200));
} }
......
source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.h
+ 8
− 4
View file @ 677eaa03
...@@ -20,7 +20,8 @@ ...@@ -20,7 +20,8 @@
*/ */
#pragma once #pragma once
#include <VirtualRobot/VirtualRobot.h>
#include "RobotAPI/interface/core/RobotState.h"
#include "RobotAPI/interface/units/KinematicUnitInterface.h" #include "RobotAPI/interface/units/KinematicUnitInterface.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h" #include "RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h"
#include <ArmarXCore/core/ManagedIceObject.h> #include <ArmarXCore/core/ManagedIceObject.h>
...@@ -49,6 +50,7 @@ namespace armarx::view_selection::skills ...@@ -49,6 +50,7 @@ namespace armarx::view_selection::skills
objpose::ObjectPoseClient objectPoseClient; objpose::ObjectPoseClient objectPoseClient;
armarx::TextListenerInterfacePrx textToSpeech; armarx::TextListenerInterfacePrx textToSpeech;
armarx::RobotStateComponentInterfacePrx robotStateComponent;
// armem::client::MemoryNameSystem& memoryNameSystem; // armem::client::MemoryNameSystem& memoryNameSystem;
}; };
...@@ -76,9 +78,11 @@ namespace armarx::view_selection::skills ...@@ -76,9 +78,11 @@ namespace armarx::view_selection::skills
Params::ToAronType()}; Params::ToAronType()};
} }
std::optional<objpose::ObjectPose> std::optional<objpose::ObjectPose>
findObject(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID); findObject(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID);
void setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num); void
void speak(const std::string& text); setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
void
speak(const std::string& text);
protected: protected:
......
source/armarx/view_selection/skills/ScanLocation.cpp
+ 78
− 20
View file @ 677eaa03
...@@ -3,15 +3,19 @@ ...@@ -3,15 +3,19 @@
#include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h" #include "RobotAPI/libraries/ArmarXObjects/forward_declarations.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h" #include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h" #include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include <VirtualRobot/Robot.h>
#include <memory> #include <memory>
#include <map> #include <map>
#include <Eigen/Core> #include <Eigen/Core>
#include <Eigen/Geometry> #include <Eigen/Geometry>
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
namespace armarx::view_selection::skills namespace armarx::view_selection::skills
{ {
std::map<std::string, float> neck_joint_angles = {
{"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}};
VirtualRobot::RobotPtr localRobot;
ScanLocation::ScanLocation() : Base(DefaultSkillDescription()) ScanLocation::ScanLocation() : Base(DefaultSkillDescription())
{ {
} }
...@@ -29,29 +33,83 @@ namespace armarx::view_selection::skills ...@@ -29,29 +33,83 @@ namespace armarx::view_selection::skills
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded}; .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
} }
void
ScanLocation::setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num)
{
float yaw_step_size = (goal_yaw - neck_joint_angles["Neck_1_Yaw"]) / waypoints_num;
float pitch_step_size = (goal_pitch - neck_joint_angles["Neck_2_Pitch"]) / waypoints_num;
localRobot->getRobotNode("AzureKinectCamera")->getGlobalPose();
for (int i = 1; i < waypoints_num; i++)
{
neck_joint_angles["Neck_1_Yaw"] = neck_joint_angles["Neck_1_Yaw"] + yaw_step_size;
neck_joint_angles["Neck_2_Pitch"] = neck_joint_angles["Neck_2_Pitch"] + pitch_step_size;
srv_->kinematicUnit->setJointAngles({
{"Neck_1_Yaw", neck_joint_angles["Neck_1_Yaw"]}, {"Neck_2_Pitch", neck_joint_angles["Neck_2_Pitch"]}});
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
void
ScanLocation::speak(const std::string& text)
{
ARMARX_INFO << "Saying '" << text.substr(0, 50) << "...'";
srv_->textToSpeech->reportText(text);
}
::armarx::skills::Skill::MainResult ::armarx::skills::Skill::MainResult
ScanLocation::main(const Base::SpecializedMainInput& in) ScanLocation::main(const Base::SpecializedMainInput& in)
{ {
// objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses(); armarx::RemoteRobot::synchronizeLocalClone(localRobot, srv_->robotStateComponent);
srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl} });
// armarx::ObjectID objectId; ARMARX_INFO << "start the running";
// std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80", // doing the neck movement routine
// "hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher", // move neck to start position
// "mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed", setNeckJointAngles(0.0, 0.2, 10);
// "mounting-adhesive", "spraybottle", "workbench"}; ARMARX_INFO << "finished first movement'";
// std::vector<std::string> all_objects_poses; // move the neck to the max right
// for (auto o : objectPoses) { setNeckJointAngles(-0.593412, 0.7609636, 10);
// all_objects_poses.push_back(o.objectID.className()); // move the neck to the max left
// } setNeckJointAngles(0.593412, 0.7609636, 20);
// std::vector<std::string> objects_names; // move the neck back to the normal
// for (auto const& object : all_objects_poses) { setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "no problem with the neck";
// getting the
objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
// these objects are always in the memory so, we need to not consider them for what can u see now scenario
armarx::ObjectID objectId;
std::vector<std::string> predefined_poses = {"r003-door", "r003-pillar", "clean-box", "dirty-box", "beam", "hund-table-120x80",
"hund-table-180x60", "hund-table-180x80", "hund-table-200x80", "mobile-dishwasher",
"mobile-fridge", "mobile-kitchen-counter", "guard-support-full-with-light", "ladder-closed",
"mounting-adhesive", "spraybottle", "workbench"};
std::vector<std::string> all_objects_poses;
for (auto o : objectPoses) {
all_objects_poses.push_back(o.objectID.className());
}
std::vector<std::string> objects_names;
for (auto const& object : all_objects_poses) {
// if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() && if (std::find(predefined_poses.begin(), predefined_poses.end(), object) == predefined_poses.end() &&
// std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) { std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
// objects_names.push_back(object); objects_names.push_back(object);
// } }
// } }
srv_->kinematicUnit->setJointAngles({{"Neck_1_Yaw", 0.0}, {"Neck_2_Pitch", 0.3}});
ARMARX_INFO << "no problem with the poses";
for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
if (i == 0) {
speak("I can see the following objects");
std::this_thread::sleep_for(std::chrono::milliseconds(200));
} else if (i == (static_cast<int>(objects_names.size()) - 1)) {
speak("and");
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
speak(objects_names[i]);
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
// ScanLocation::setJointAngles(); // ScanLocation::setJointAngles();
return ::armarx::skills::Skill::MainResult{ return ::armarx::skills::Skill::MainResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded}; .status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
......
source/armarx/view_selection/skills/ScanLocation.h
+ 12
− 1
View file @ 677eaa03
...@@ -21,6 +21,7 @@ ...@@ -21,6 +21,7 @@
#pragma once #pragma once
#include "RobotAPI/interface/core/RobotState.h"
#include "RobotAPI/interface/units/KinematicUnitInterface.h" #include "RobotAPI/interface/units/KinematicUnitInterface.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h" #include "RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h"
#include <ArmarXCore/core/ManagedIceObject.h> #include <ArmarXCore/core/ManagedIceObject.h>
...@@ -32,6 +33,7 @@ ...@@ -32,6 +33,7 @@
#include <armarx/view_selection/skills/aron/ScanLocation.aron.generated.h> #include <armarx/view_selection/skills/aron/ScanLocation.aron.generated.h>
#include <armarx/view_selection/skills/constants.h> #include <armarx/view_selection/skills/constants.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
namespace armarx::view_selection::skills namespace armarx::view_selection::skills
{ {
class ScanLocation : class ScanLocation :
...@@ -47,6 +49,10 @@ namespace armarx::view_selection::skills ...@@ -47,6 +49,10 @@ namespace armarx::view_selection::skills
KinematicUnitInterfacePrx kinematicUnit; KinematicUnitInterfacePrx kinematicUnit;
objpose::ObjectPoseClient objectPoseClient; objpose::ObjectPoseClient objectPoseClient;
armarx::TextListenerInterfacePrx textToSpeech;
armarx::RobotStateComponentInterfacePrx robotStateComponent;
// armem::client::MemoryNameSystem& memoryNameSystem; // armem::client::MemoryNameSystem& memoryNameSystem;
}; };
...@@ -73,7 +79,12 @@ namespace armarx::view_selection::skills ...@@ -73,7 +79,12 @@ namespace armarx::view_selection::skills
armarx::core::time::Duration(), armarx::core::time::Duration(),
Params::ToAronType()}; Params::ToAronType()};
} }
void setJointAngles(); void
setJointAngles();
void
setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
void
speak(const std::string& text);
protected: protected:
private: private:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment