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

complete working draft for LookForObjectWithKinematicUnit

parent 5b72c159
No related branches found
No related tags found
1 merge request!16Feature/skills
Hide whitespace changes
Inline Side-by-side
Showing
with 152 additions and 144 deletions
source/armarx/view_selection/components/view_selection_skill_provider/Component.cpp
+ 5
− 1
View file @ c91b08bb
......@@ -69,6 +69,8 @@ namespace armarx::view_selection::components::view_selection_skill_provider
// Add an optionalproperty.
def->topic(this->textToSpeech, "TextToSpeech");
return def;
}
......@@ -112,8 +114,10 @@ namespace armarx::view_selection::components::view_selection_skill_provider
skills::LookForObjectWithKinematicUnit::Services srv{
.kinematicUnit = getRobotUnit()->getKinematicUnit(),
.objectPoseClient = getClient()
.objectPoseClient = getClient(),
.textToSpeech = this->textToSpeech
};
skills_.lookForObjectWithKinematicUnit->connect(srv);
}
......
source/armarx/view_selection/components/view_selection_skill_provider/Component.h
+ 5
− 0
View file @ c91b08bb
......@@ -34,6 +34,8 @@
#include <RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h>
#include <RobotAPI/libraries/skills/provider/SkillProviderComponentPlugin.h>
#include <RobotAPI/interface/speech/SpeechInterface.h>
// #include <armarx/view_selection/components/view_selection_skill_provider/ComponentInterface.h>
namespace armarx::view_selection::components::view_selection_skill_provider
......@@ -120,6 +122,9 @@ namespace armarx::view_selection::components::view_selection_skill_provider
{
};
Properties properties;
armarx::TextListenerInterfacePrx textToSpeech;
/* Use a mutex if you access variables from different threads
* (e.g. ice functions and RemoteGui_update()).
std::mutex propertiesMutex;
......
source/armarx/view_selection/skills/LookForObjectWithKinematicUnit.cpp
+ 55
− 64
View file @ c91b08bb
#include "LookForObjectWithKinematicUnit.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectFinder.h"
#include "RobotAPI/libraries/ArmarXObjects/ObjectInfo.h"
#include "RobotAPI/libraries/ArmarXObjects/aron_conversions.h"
#include "RobotAPI/libraries/aron/core/aron_conversions.h"
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
......@@ -10,14 +11,17 @@
#include <Eigen/Geometry>
#include <chrono>
#include <optional>
#include <thread>
#include <vector>
namespace armarx::view_selection::skills
{
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())
{
}
......@@ -34,42 +38,24 @@ namespace armarx::view_selection::skills
return ::armarx::skills::Skill::InitResult{
.status = ::armarx::skills::TerminatedSkillStatus::Succeeded};
}
// std::optional<objpose::ObjectPose>
// findObjectByClass(const objpose::ObjectPoseSeq& objectPoses, const armarx::ObjectID& objectID)
// {
// const auto matchesId = [&objectID](const objpose::ObjectPose& objectPose) -> bool
// { return objectPose.objectID.equalClass(objectID); };
// const auto it = std::find_if(objectPoses.begin(), objectPoses.end(), matchesId);
// if (it != objectPoses.end())
// {
// return *it;
// }
// return std::nullopt;
// }
void
LookForObjectWithKinematicUnit::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;
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));
// 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(300));
}
std::this_thread::sleep_for(std::chrono::milliseconds(200));
//armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(200));
std::this_thread::sleep_for(std::chrono::milliseconds(300));
}
void
LookForObjectWithKinematicUnit::speak(const std::string& text)
......@@ -81,40 +67,26 @@ namespace armarx::view_selection::skills
::armarx::skills::Skill::MainResult
LookForObjectWithKinematicUnit::main(const Base::SpecializedMainInput& in)
{
// TODO(Nagi): implement
// objpose::ObjectPoseSeq objectPoses = srv_->objectPoseClient.fetchObjectPoses();
// armarx::ObjectID objectId;
// fromAron(in.params.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() &&
// std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
// objects_names.push_back(object);
// }
// }
// 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} });
srv_->kinematicUnit->switchControlMode({{"Neck_1_Yaw", armarx::ControlMode::ePositionControl},{"Neck_2_Pitch", armarx::ControlMode::ePositionControl}, {"TorsoJoint", armarx::ControlMode::ePositionControl} });
srv_->kinematicUnit->setJointAngles({
{"TorsoJoint", -109.76679992675781}});
std::this_thread::sleep_for(std::chrono::milliseconds(300));
ARMARX_INFO << "start the running";
// doing the neck movement routine
// move neck to start position
setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "finished first movement'";
// move the neck to the max right
setNeckJointAngles(-0.593412, 0.7609636, 10);
setNeckJointAngles(-0.593412, 0.66654124, 10);
std::this_thread::sleep_for(std::chrono::milliseconds(450));
// move the neck to the max left
setNeckJointAngles(0.593412, 0.7609636, 20);
setNeckJointAngles(0.0, 0.66654124, 10);
std::this_thread::sleep_for(std::chrono::milliseconds(450));
setNeckJointAngles(0.593412, 0.66654124, 10);
std::this_thread::sleep_for(std::chrono::milliseconds(450));
// move the neck back to the normal
setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "no problem with the neck";
......@@ -126,22 +98,41 @@ namespace armarx::view_selection::skills
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());
}
"mounting-adhesive", "spraybottle", "workbench", "box"};
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() &&
std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
objects_names.push_back(object);
armarx::ObjectFinder objFind;
for (auto o : objectPoses) {
const std::string className = o.objectID.className();
if (std::find(predefined_poses.begin(), predefined_poses.end(), className) == predefined_poses.end() &&
std::find(objects_names.begin(), objects_names.end(), className) == objects_names.end())
{
std::string objectName;
std::optional<armarx::ObjectInfo> info = objFind.findObject(o.objectID);
if (info)
{
std::optional<std::vector<std::string>> names = info->loadSpokenNames();
if (names.has_value() and not names->empty())
{
objectName = names->front();
}
}
if (objectName.empty())
{
objectName = className;
}
objects_names.push_back(objectName);
}
}
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");
......
source/armarx/view_selection/skills/ScanLocation.cpp
+ 80
− 72
View file @ c91b08bb
......@@ -13,9 +13,9 @@
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;
// 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())
{
}
......@@ -33,81 +33,89 @@ namespace armarx::view_selection::skills
.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;
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);
}
// 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;
// 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
ScanLocation::main(const Base::SpecializedMainInput& in)
{
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
// move neck to start position
setNeckJointAngles(0.0, 0.2, 10);
ARMARX_INFO << "finished first movement'";
// move the neck to the max right
setNeckJointAngles(-0.593412, 0.7609636, 10);
// move the neck to the max left
setNeckJointAngles(0.593412, 0.7609636, 20);
// move the neck back to the normal
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) {
// 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
// // move neck to start position
// setNeckJointAngles(0.0, 0.2, 10);
// ARMARX_INFO << "finished first movement'";
// // move the neck to the max right
// setNeckJointAngles(-0.593412, 0.7609636, 10);
// // move the neck to the max left
// setNeckJointAngles(0.593412, 0.7609636, 20);
// // move the neck back to the normal
// 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() &&
std::find(objects_names.begin(), objects_names.end(), object) == objects_names.end()) {
objects_names.push_back(object);
}
}
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));
}
// 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()) {
// objects_names.push_back(object);
// }
// }
// ARMARX_INFO << "no problem with the poses";
// for (int i = 0; i < static_cast<int>(objects_names.size()); i++) {
// if (i == 0) {
// if (static_cast<int>(objects_names.size()) == 1) {
// speak("I can see the following object");
// std::this_thread::sleep_for(std::chrono::milliseconds(200));
// }
// else{
// 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();
return ::armarx::skills::Skill::MainResult{
......
source/armarx/view_selection/skills/ScanLocation.h
+ 7
− 7
View file @ c91b08bb
......@@ -48,11 +48,11 @@ namespace armarx::view_selection::skills
{
KinematicUnitInterfacePrx kinematicUnit;
objpose::ObjectPoseClient objectPoseClient;
// objpose::ObjectPoseClient objectPoseClient;
armarx::TextListenerInterfacePrx textToSpeech;
// armarx::TextListenerInterfacePrx textToSpeech;
armarx::RobotStateComponentInterfacePrx robotStateComponent;
// armarx::RobotStateComponentInterfacePrx robotStateComponent;
// armem::client::MemoryNameSystem& memoryNameSystem;
};
......@@ -81,10 +81,10 @@ namespace armarx::view_selection::skills
}
void
setJointAngles();
void
setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
void
speak(const std::string& text);
// void
// setNeckJointAngles(float goal_yaw, float goal_pitch, int waypoints_num);
// void
// speak(const std::string& text);
protected:
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