From 27876dabf4d3f03b482ab176f7b561ae776dabb0 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 5 Jul 2021 20:47:45 +0200
Subject: [PATCH 01/14] first draft
---
VirtualRobot/Workspace/NaturalPosture.cpp | 243 ++++++++++++++++++++++
VirtualRobot/Workspace/NaturalPosture.h | 77 +++++++
2 files changed, 320 insertions(+)
create mode 100644 VirtualRobot/Workspace/NaturalPosture.cpp
create mode 100644 VirtualRobot/Workspace/NaturalPosture.h
diff --git a/VirtualRobot/Workspace/NaturalPosture.cpp b/VirtualRobot/Workspace/NaturalPosture.cpp
new file mode 100644
index 000000000..e5a99a594
--- /dev/null
+++ b/VirtualRobot/Workspace/NaturalPosture.cpp
@@ -0,0 +1,243 @@
+#include "NaturalPosture.h"
+
+#include <cfloat>
+#include <climits>
+#include <cmath>
+#include <fstream>
+#include <stdexcept>
+
+#include <VirtualRobot/IK/CompositeDiffIK/Soechting.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+#include "../Grasping/Grasp.h"
+#include "../Grasping/GraspSet.h"
+#include "../ManipulationObject.h"
+#include "../Robot.h"
+#include "../RobotNodeSet.h"
+#include "../VirtualRobotException.h"
+#include "IK/CompositeDiffIK/CompositeDiffIK.h"
+#include "IK/CompositeDiffIK/SoechtingNullspaceGradient.h"
+
+namespace VirtualRobot
+{
+
+ NaturalPosture::NaturalPosture(RobotPtr robot) : WorkspaceRepresentation(robot)
+ {
+ type = "NaturalPosture";
+ }
+
+ void NaturalPosture::customInitialize()
+ {
+ // auto target1 = ik->addTarget(robot->getRobotNode(tcpNode->getName()), VirtualRobot::IKSolver::All);
+ }
+
+ float NaturalPosture::evaluate()
+ {
+ SoechtingNullspaceGradientPtr soechtingNullspaceGradient;
+
+ CompositeDiffIK::Parameters params;
+
+ const CompositeDiffIK::TargetPtr target(new CompositeDiffIK::Target(
+ tcpNode, tcpNode->getGlobalPose(), VirtualRobot::IKSolver::All));
+
+ if (robot->getName() == "Armar6" && nodeSet->getName() == "RightArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmR5_Elb1");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradient(
+ target, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
+ soechtingNullspaceGradient->kP = 1.0;
+ // ik->addNullspaceGradient(gradient);
+ }
+ else if (robot->getName() == "Armar6" && nodeSet->getName() == "LeftArm")
+ {
+ std::cout << "Adding soechting nullspace" << std::endl;
+ VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmL5_Elb1");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradient(
+ target, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
+ soechtingNullspaceGradient->kP = 1.0;
+
+ // ik->addNullspaceGradient(gradient);
+ }
+ else
+ {
+ throw std::runtime_error("Unknown robot");
+ }
+
+ const auto weightedDiff = soechtingNullspaceGradient->getGradient(params, -1);
+
+ return weightedDiff.norm();
+ }
+
+ void NaturalPosture::addPose(const Eigen::Matrix4f& pose)
+ {
+ Eigen::Matrix4f p = pose;
+ toLocal(p);
+
+ float x[6];
+
+ matrix2Vector(p, x);
+ //MathTools::eigen4f2rpy(p,x);
+
+ // check for achieved values
+ for (int i = 0; i < 6; i++)
+ {
+ if (x[i] < achievedMinValues[i])
+ {
+ achievedMinValues[i] = x[i];
+ }
+
+ if (x[i] > achievedMaxValues[i])
+ {
+ achievedMaxValues[i] = x[i];
+ }
+ }
+
+ // get voxels
+ unsigned int v[6];
+
+ if (getVoxelFromPose(x, v))
+ {
+
+ // float m = getCurrentManipulability(qualMeasure, selfDistSt, selfDistDyn);
+ // float mSc = m / maxManip;
+
+ // if (mSc > 1)
+ // {
+ // if (mSc > 1.05)
+ // {
+ // VR_WARNING << "Manipulability is larger than max value. Current Manip:" << m << ", maxManip:" << maxManip << ", percent:" << mSc << std::endl;
+ // }
+
+ // mSc = 1.0f;
+ // }
+
+ // if (m < 0)
+ // {
+ // mSc = 0;
+ // }
+
+ // unsigned char e = (unsigned char)(mSc * (float)UCHAR_MAX + 0.5f);
+
+
+
+ //cout<<"m = "<<m<<endl;
+ //cout<<"mSc = "<<mSc<<endl;
+ //cout<<"e = "<<int(e)<<endl;
+
+ const float e = evaluate();
+
+ // add at least 1, since the pose is reachable
+ if (e == 0)
+ {
+ e = 1;
+ }
+
+ if (e > data->get(v))
+ {
+ data->setDatum(v, e);
+ }
+ }
+
+ buildUpLoops++;
+ }
+
+ /*
+ void NaturalPosture::addRandomTCPPoses(unsigned int loops, bool checkForSelfCollisions)
+ {
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "NaturalPosture data not initialized");
+
+ std::vector<float> c;
+ nodeSet->getJointValues(c);
+ bool visuSate = robot->getUpdateVisualizationStatus();
+ robot->setUpdateVisualization(false);
+
+ for (unsigned int i = 0; i < loops; i++)
+ {
+ if (setRobotNodesToRandomConfig(nodeSet, checkForSelfCollisions))
+ {
+ addCurrentTCPPose();
+ }
+ else
+ {
+ VR_WARNING << "Could not find collision-free configuration...";
+ }
+ }
+
+ robot->setUpdateVisualization(visuSate);
+ robot->setJointValues(nodeSet, c);
+ }*/
+
+ // bool NaturalPosture::isReachable(const Eigen::Matrix4f& globalPose)
+ // {
+ // return isCovered(globalPose);
+ // }
+
+ // VirtualRobot::GraspSetPtr NaturalPosture::getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object)
+ // {
+ // THROW_VR_EXCEPTION_IF(!object, "no object");
+ // THROW_VR_EXCEPTION_IF(!grasps, "no grasps");
+
+ // GraspSetPtr result(new GraspSet(grasps->getName(), grasps->getRobotType(), grasps->getEndEffector()));
+
+ // for (unsigned int i = 0; i < grasps->getSize(); i++)
+ // {
+ // Eigen::Matrix4f m = grasps->getGrasp(i)->getTcpPoseGlobal(object->getGlobalPose());
+
+ // if (isReachable(m))
+ // {
+ // result->addGrasp(grasps->getGrasp(i));
+ // }
+ // }
+
+ // return result;
+ // }
+
+ // Eigen::Matrix4f NaturalPosture::sampleReachablePose()
+ // {
+ // return sampleCoveredPose();
+ // }
+
+ // VirtualRobot::WorkspaceRepresentationPtr NaturalPosture::clone()
+ // {
+ // VirtualRobot::NaturalPosturePtr res(new NaturalPosture(robot));
+ // res->setOrientationType(this->orientationType);
+ // res->versionMajor = this->versionMajor;
+ // res->versionMinor = this->versionMinor;
+ // res->nodeSet = this->nodeSet;
+ // res->type = this->type;
+
+ // res->baseNode = this->baseNode;
+ // res->tcpNode = this->tcpNode;
+ // res->staticCollisionModel = this->staticCollisionModel;
+ // res->dynamicCollisionModel = this->dynamicCollisionModel;
+ // res->buildUpLoops = this->buildUpLoops;
+ // res->collisionConfigs = this->collisionConfigs;
+ // res->discretizeStepTranslation = this->discretizeStepTranslation;
+ // res->discretizeStepRotation = this->discretizeStepRotation;
+ // memcpy(res->minBounds, this->minBounds, sizeof(float) * 6);
+ // memcpy(res->maxBounds, this->maxBounds, sizeof(float) * 6);
+ // memcpy(res->numVoxels, this->numVoxels, sizeof(float) * 6);
+ // memcpy(res->achievedMinValues, this->achievedMinValues, sizeof(float) * 6);
+ // memcpy(res->achievedMaxValues, this->achievedMaxValues, sizeof(float) * 6);
+ // memcpy(res->spaceSize, this->spaceSize, sizeof(float) * 6);
+
+ // res->adjustOnOverflow = this->adjustOnOverflow;
+ // res->data.reset(this->data->clone());
+
+ // return res;
+ // }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/NaturalPosture.h b/VirtualRobot/Workspace/NaturalPosture.h
new file mode 100644
index 000000000..ca1e335cf
--- /dev/null
+++ b/VirtualRobot/Workspace/NaturalPosture.h
@@ -0,0 +1,77 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with this program. If not, see <http://www.gnu.org/licenses/>.
+*
+* @package VirtualRobot
+* @author Peter Kaiser, Nikolaus Vahrenkamp
+* @copyright 2011 Peter Kaiser, Nikolaus Vahrenkamp
+* GNU Lesser General Public License
+*
+*/
+#pragma once
+
+#include "../VirtualRobot.h"
+#include "WorkspaceRepresentation.h"
+
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+
+namespace VirtualRobot
+{
+
+
+ class VIRTUAL_ROBOT_IMPORT_EXPORT NaturalPosture : public WorkspaceRepresentation, public std::enable_shared_from_this<NaturalPosture>
+ {
+ public:
+ friend class CoinVisualizationFactory;
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ NaturalPosture(RobotPtr robot);
+
+ void customInitialize() override;
+
+
+ void addPose(const Eigen::Matrix4f& pose) override;
+
+ // /*!
+ // Returns true, if the corresponding NaturalPosture entry is non zero.
+ // */
+ // bool isReachable(const Eigen::Matrix4f& globalPose);
+
+ // /*!
+ // Returns all reachable grasps that can be applied at the current position of object.
+ // */
+ // GraspSetPtr getReachableGrasps(GraspSetPtr grasps, ManipulationObjectPtr object);
+
+
+ // //! returns a random pose that is covered by the workspace data.
+ // Eigen::Matrix4f sampleReachablePose();
+
+ // /*!
+ // Creates a deep copy of this data structure. A NaturalPosturePtr is returned.
+ // */
+ // WorkspaceRepresentationPtr clone() override;
+
+
+
+ protected:
+
+ float evaluate();
+
+ };
+
+
+} // namespace VirtualRobot
+
--
GitLab
From 3ff1f34c475fe6af366b7ac79a7c354aa75f195c Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 6 Jul 2021 08:10:10 +0200
Subject: [PATCH 02/14] draft
---
VirtualRobot/IK/CompositeDiffIK/Soechting.h | 2 +
.../SoechtingNullspaceGradient.cpp | 193 ++++++++++--------
.../SoechtingNullspaceGradient.h | 1 +
.../IK/platform/MecanumPlatformKinematics.cpp | 68 ++++++
VirtualRobot/Workspace/WorkspaceGrid.cpp | 25 ++-
.../Workspace/WorkspaceRepresentation.cpp | 9 +-
.../reachability/reachabilityScene.cpp | 1 +
.../reachability/reachabilityWindow.cpp | 21 +-
8 files changed, 224 insertions(+), 96 deletions(-)
create mode 100644 VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp
diff --git a/VirtualRobot/IK/CompositeDiffIK/Soechting.h b/VirtualRobot/IK/CompositeDiffIK/Soechting.h
index 03ed4ef4f..971ae6875 100644
--- a/VirtualRobot/IK/CompositeDiffIK/Soechting.h
+++ b/VirtualRobot/IK/CompositeDiffIK/Soechting.h
@@ -64,10 +64,12 @@ struct Soechting
soechtingTarget(0) *= -1;
}
+ // spherical coordinates: distance, azimuth, elevation
const float R = soechtingTarget.norm();
const float Chi = MathTools::rad2deg(std::atan2(soechtingTarget.x(), soechtingTarget.y()));
const float Psi = MathTools::rad2deg(std::atan2(soechtingTarget.z(), soechtingTarget.head<2>().norm()));
+ // Kondo, Koichi. “Inverse Kinematics of a Human Arm,†n.d., 16.
SoechtingAngles sa;
if (accuratePointing) {
// Angles derived from accurate pointing
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
index 533b14904..04f7bba22 100644
--- a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
@@ -17,98 +17,125 @@
*/
#include "SoechtingNullspaceGradient.h"
-#include "Soechting.h"
-#include <VirtualRobot/Robot.h>
+
+#include <cmath>
+
#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+
+#include "MathTools.h"
+#include "Soechting.h"
+#include "VirtualRobot.h"
namespace VirtualRobot
{
-SoechtingNullspaceGradient::SoechtingNullspaceGradient(const CompositeDiffIK::TargetPtr& target, const std::string& shoulderName, const Soechting::ArmType &arm, const ArmJoints& joints) :
- CompositeDiffIK::NullspaceGradient(joints.getRobotNodeNames()),
- rns(joints.createRobotNodeSet("Soechting" + std::to_string(arm))),
- target(target),
- shoulder(rns->getRobot()->getRobotNode(shoulderName)),
- arm(arm),
- joints(joints)
-{
-}
-
-void SoechtingNullspaceGradient::init(CompositeDiffIK::Parameters &) {
- // Do nothing
-}
-
-Eigen::VectorXf SoechtingNullspaceGradient::getGradient(CompositeDiffIK::Parameters ¶ms, int /*stepNr*/) {
- const size_t dim = rns->getSize();
+ SoechtingNullspaceGradient::SoechtingNullspaceGradient(const CompositeDiffIK::TargetPtr& target,
+ const std::string& shoulderName,
+ const Soechting::ArmType& arm,
+ const ArmJoints& joints) :
+ CompositeDiffIK::NullspaceGradient(joints.getRobotNodeNames()),
+ rns(joints.createRobotNodeSet("Soechting" + std::to_string(arm))),
+ target(target),
+ shoulder(rns->getRobot()->getRobotNode(shoulderName)),
+ arm(arm),
+ joints(joints)
+ {
+ }
- auto sa = calcShoulderAngles(params);
- Eigen::VectorXf weights = Eigen::VectorXf::Zero(dim);
- Eigen::VectorXf target = Eigen::VectorXf::Zero(dim);
+ void SoechtingNullspaceGradient::init(CompositeDiffIK::Parameters&)
+ {
+ // Do nothing
+ }
-#define SET(joint, w, t)\
- if (joint)\
- {\
- auto id = rns->getRobotNodeIndex(joint);\
- weights[id] = w;\
- target[id] = t;\
+ Eigen::VectorXf SoechtingNullspaceGradient::getGradient(CompositeDiffIK::Parameters& params,
+ int /*stepNr*/)
+ {
+ const size_t dim = rns->getSize();
+
+ auto sa = calcShoulderAngles(params);
+ Eigen::VectorXf weights = Eigen::VectorXf::Zero(dim);
+ Eigen::VectorXf target = Eigen::VectorXf::Zero(dim);
+
+ const auto set = [&](const auto& joint, const float w, const float t)
+ {
+ if (joint)
+ {
+ auto id = rns->getRobotNodeIndex(joint);
+ VR_ASSERT(id >= 0);
+ weights[id] = w;
+ target[id] = t;
+ }
+ };
+
+ set(joints.clavicula, 0.5f, sa.C);
+ set(joints.shoulder1, 1, sa.SE);
+ set(joints.shoulder2, 2, sa.SR),
+ set(joints.shoulder3, 0.5f, -M_PI / 4);
+ set(joints.elbow, 0.5f, sa.E);
+
+
+ // std::cout << "target" << target << std::endl;
+ // std::cout << "diff" << (target - rns->getJointValuesEigen()) << std::endl;
+
+ return (target - rns->getJointValuesEigen()).cwiseProduct(weights);
}
- SET(joints.clavicula, 0.5f, sa.C)
- SET(joints.shoulder1, 1, sa.SE)
- SET(joints.shoulder2, 2, sa.SR)
- SET(joints.shoulder3, 0.5f, -M_PI / 4)
- SET(joints.elbow, 0.5f, sa.E)
-#undef SET
- std::cout << target << std::endl;
-
- return (target - rns->getJointValuesEigen()).cwiseProduct(weights);
-}
-
-SoechtingNullspaceGradient::ShoulderAngles SoechtingNullspaceGradient::calcShoulderAngles(const CompositeDiffIK::Parameters& /*params*/) const
-{
- const Eigen::Matrix4f currentShoulder = shoulder->getPoseInRootFrame();
-
- Soechting::Arm arm;
- arm.armType = this->arm;
- arm.shoulder = currentShoulder.block<3, 1>(0, 3);
- arm.upperArmLength = 0.f;
- arm.forearmLength = 0.f;
-
- Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
- const auto sa = Soechting::CalculateAngles(targetPosition, arm);
-
- const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
- const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
- const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
- const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
-
- const Eigen::Vector3f elb = Eigen::AngleAxisf(-sa.SE, Eigen::Vector3f::UnitX())
- * aaSY * aaSE * -Eigen::Vector3f::UnitZ();
- const float SR = std::atan2(elb(0), -elb(2));
-
- ShoulderAngles res;
- res.SR = std::max(-0.1f, SR);
- res.SE = sa.SE;
- res.E = sa.EE;
- res.C = 0;
- return res;
-}
-
-RobotNodeSetPtr SoechtingNullspaceGradient::ArmJoints::createRobotNodeSet(const std::string &name) const {
- std::vector<RobotNodePtr> robotNodes;
- for (auto node : { clavicula, shoulder1, shoulder2, shoulder3, elbow }) {
- if (node) robotNodes.push_back(node);
+
+ SoechtingNullspaceGradient::ShoulderAngles SoechtingNullspaceGradient::calcShoulderAngles(
+ const CompositeDiffIK::Parameters& /*params*/) const
+ {
+ const Eigen::Matrix4f currentShoulder = shoulder->getPoseInRootFrame();
+
+ Soechting::Arm arm;
+ arm.armType = this->arm;
+ arm.shoulder = currentShoulder.block<3, 1>(0, 3);
+ arm.upperArmLength = 0.f;
+ arm.forearmLength = 0.f;
+
+ Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
+ const auto sa = Soechting::CalculateAngles(targetPosition, arm, 1.0, true);
+
+ const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+
+ const Eigen::Vector3f elb = Eigen::AngleAxisf(-sa.SE, Eigen::Vector3f::UnitX()) * aaSY *
+ aaSE * -Eigen::Vector3f::UnitZ();
+ const float SR = std::atan2(elb(0), -elb(2));
+
+ ShoulderAngles res;
+ res.SR = std::max(-0.1f, SR);
+ res.SE = sa.SE;
+ res.E = sa.EE;
+ res.C = 0;
+ return res;
}
- if (robotNodes.empty()) return nullptr;
- auto frontNode = robotNodes.front();
- return RobotNodeSet::createRobotNodeSet(frontNode->getRobot(), name, robotNodes, frontNode);
-}
+ RobotNodeSetPtr
+ SoechtingNullspaceGradient::ArmJoints::createRobotNodeSet(const std::string& name) const
+ {
+ std::vector<RobotNodePtr> robotNodes;
+ for (auto node : {clavicula, shoulder1, shoulder2, shoulder3, elbow})
+ {
+ if (node)
+ robotNodes.push_back(node);
+ }
+ if (robotNodes.empty())
+ return nullptr;
+
+ auto frontNode = robotNodes.front();
+ return RobotNodeSet::createRobotNodeSet(frontNode->getRobot(), name, robotNodes, frontNode);
+ }
-std::vector<std::string> SoechtingNullspaceGradient::ArmJoints::getRobotNodeNames() const {
- std::vector<std::string> nodeNames;
- for (auto node : { clavicula, shoulder1, shoulder2, shoulder3, elbow }) {
- if (node) nodeNames.push_back(node->getName());
+ std::vector<std::string> SoechtingNullspaceGradient::ArmJoints::getRobotNodeNames() const
+ {
+ std::vector<std::string> nodeNames;
+ for (auto node : {clavicula, shoulder1, shoulder2, shoulder3, elbow})
+ {
+ if (node)
+ nodeNames.push_back(node->getName());
+ }
+ return nodeNames;
}
- return nodeNames;
-}
-}
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
index 493082cf3..63c217cf2 100644
--- a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h
@@ -24,6 +24,7 @@
namespace VirtualRobot
{
+
class SoechtingNullspaceGradient : public CompositeDiffIK::NullspaceGradient
{
public:
diff --git a/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp b/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp
new file mode 100644
index 000000000..a5104d414
--- /dev/null
+++ b/VirtualRobot/IK/platform/MecanumPlatformKinematics.cpp
@@ -0,0 +1,68 @@
+
+
+#include "MecanumPlatformKinematics.h"
+
+namespace VirtualRobot
+{
+ Eigen::Matrix<float, 4, 3> MecanumPlatformKinematicsParams::J_inv() const
+ {
+ const float k = l1 + l2;
+
+ Eigen::Matrix<float, 4, 3> m;
+ // clang-format off
+ m << 1, 1, -k,
+ -1, 1, k,
+ -1, 1, -k,
+ 1, 1, k;
+ // clang-format on
+
+ m *= 1 / R;
+
+ return m;
+ }
+
+ Eigen::Matrix<float, 3, 4> MecanumPlatformKinematicsParams::J() const
+ {
+ const float k = 1 / (l1 + l2);
+
+ Eigen::Matrix<float, 3, 4> m;
+
+ // according to the paper
+ // m << -1, 1, 1, -1,
+ // 1, 1, 1, 1,
+ // -k, k, -k, k;
+
+ // "ours"
+ // clang-format off
+ m << 1, -1, -1, 1,
+ 1, 1, 1, 1,
+ -k, k, -k, k;
+ // clang-format on
+
+ m *= R / 4;
+
+ return m;
+ }
+
+ MecanumPlatformKinematics::MecanumPlatformKinematics(const Params& params) : params(params)
+ {
+ }
+
+ MecanumPlatformKinematics::WheelVelocities
+ MecanumPlatformKinematics::calcWheelVelocity(const CartesianVelocity& v)
+ {
+ return J_inv * v;
+ }
+
+ MecanumPlatformKinematics::CartesianVelocity
+ MecanumPlatformKinematics::calcCartesianVelocity(const WheelVelocities& w)
+ {
+ return J * w;
+ }
+
+ const MecanumPlatformKinematics::Params& MecanumPlatformKinematics::getParams() const
+ {
+ return params;
+ }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Workspace/WorkspaceGrid.cpp b/VirtualRobot/Workspace/WorkspaceGrid.cpp
index 887a34581..865f907ea 100644
--- a/VirtualRobot/Workspace/WorkspaceGrid.cpp
+++ b/VirtualRobot/Workspace/WorkspaceGrid.cpp
@@ -253,17 +253,17 @@ namespace VirtualRobot
setCellEntry(nPosX, nPosY, value, grasp);
- if (nPosX > 0 && nPosX < (gridSizeX - 1) && nPosY > 0 && nPosY < (gridSizeY - 1))
- {
- setCellEntry(nPosX - 1, nPosY, value, grasp);
- setCellEntry(nPosX - 1, nPosY - 1, value, grasp);
- setCellEntry(nPosX - 1, nPosY + 1, value, grasp);
- setCellEntry(nPosX, nPosY - 1, value, grasp);
- setCellEntry(nPosX, nPosY + 1, value, grasp);
- setCellEntry(nPosX + 1, nPosY - 1, value, grasp);
- setCellEntry(nPosX + 1, nPosY, value, grasp);
- setCellEntry(nPosX + 1, nPosY + 1, value, grasp);
- }
+ // if (nPosX > 0 && nPosX < (gridSizeX - 1) && nPosY > 0 && nPosY < (gridSizeY - 1))
+ // {
+ // setCellEntry(nPosX - 1, nPosY, value, grasp);
+ // setCellEntry(nPosX - 1, nPosY - 1, value, grasp);
+ // setCellEntry(nPosX - 1, nPosY + 1, value, grasp);
+ // setCellEntry(nPosX, nPosY - 1, value, grasp);
+ // setCellEntry(nPosX, nPosY + 1, value, grasp);
+ // setCellEntry(nPosX + 1, nPosY - 1, value, grasp);
+ // setCellEntry(nPosX + 1, nPosY, value, grasp);
+ // setCellEntry(nPosX + 1, nPosY + 1, value, grasp);
+ // }
}
void WorkspaceGrid::setEntries(std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr>& wsData, const Eigen::Matrix4f& graspGlobal, GraspPtr grasp)
@@ -281,6 +281,9 @@ namespace VirtualRobot
x = tmpPos2(0, 3);
y = tmpPos2(1, 3);
+ // setCellEntry(x, y, i->value, grasp);
+
+
setEntryCheckNeighbors(x, y, i->value, grasp);
//setEntry(x,y,vData[i].value);
}
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index be56bba8c..47b776ed7 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -938,6 +938,8 @@ namespace VirtualRobot
void WorkspaceRepresentation::addPose(const Eigen::Matrix4f& globalPose)
{
+ VR_INFO << "oops";
+
VR_ASSERT(data);
Eigen::Matrix4f p = globalPose;
toLocal(p);
@@ -1137,7 +1139,7 @@ namespace VirtualRobot
for (int i = 0; i < 6; i++)
{
- THROW_VR_EXCEPTION_IF(minBounds[i] >= maxBounds[i], "Min/MaxBound error");
+ THROW_VR_EXCEPTION_IF(minBounds[i] >= maxBounds[i], "Min/Max bound error");
}
THROW_VR_EXCEPTION_IF(!nodeSet, "NULL data, need a nodeSet");
@@ -1847,6 +1849,8 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No reachability data loaded");
Eigen::Matrix4f p = tcpNode->getGlobalPose();
+
+ VR_INFO << "Adding pose";
addPose(p);
}
@@ -2233,6 +2237,9 @@ namespace VirtualRobot
if (successfullyRandomized)
{
Eigen::Matrix4f p = clonedTcpNode->getGlobalPose();
+
+ VR_INFO << "Adding pose";
+ std::cout << "Adding pose";
addPose(p);
}
else
diff --git a/VirtualRobot/examples/reachability/reachabilityScene.cpp b/VirtualRobot/examples/reachability/reachabilityScene.cpp
index c66d17fc5..3b68353a7 100644
--- a/VirtualRobot/examples/reachability/reachabilityScene.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityScene.cpp
@@ -73,6 +73,7 @@ void endlessExtend(std::string robotFile, std::string reachFile, int steps, unsi
WorkspaceRepresentationPtr reachSpace;
bool loadOK = true;
+
// try manipulability file
try
{
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index ed0b8b445..7ecf31c79 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -3,6 +3,7 @@
#include "VirtualRobot/EndEffector/EndEffector.h"
#include "VirtualRobot/Workspace/Reachability.h"
#include "VirtualRobot/Workspace/Manipulability.h"
+#include "VirtualRobot/Workspace/NaturalPosture.h"
#include "VirtualRobot/IK/PoseQualityExtendedManipulability.h"
#include "VirtualRobot/XML/RobotIO.h"
#include "VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h"
@@ -493,10 +494,14 @@ void reachabilityWindow::extendReach()
}
int steps = UI.spinBoxExtend->value();
+ VR_INFO << "Adding " << steps << " random TCP poses.";
//reachSpace->addRandomTCPPoses(steps, 1, true);
reachSpace->addRandomTCPPoses(steps, QThread::idealThreadCount() < 1 ? 1 : QThread::idealThreadCount(), true);
+ VR_INFO << "Done adding random TCP poses.";
+
+
reachSpace->print();
UI.checkBoxReachabilityVisu->setChecked(false);
UI.sliderCutReach->setEnabled(false);
@@ -582,13 +587,22 @@ void reachabilityWindow::createReach()
std::string measure = std::string(UICreate.comboBoxQualityMeasure->currentText().toLatin1());
- if (measure != "Reachability")
+ measure = "NaturalPosture";
+
+ if(measure == "NaturalPosture")
+ {
+ reachSpace.reset(new NaturalPosture(robot));
+ }
+ else if (measure != "Reachability")
{
reachSpace.reset(new Manipulability(robot));
ManipulabilityPtr manipSpace = std::dynamic_pointer_cast<Manipulability>(reachSpace);
manipSpace->setMaxManipulability(UICreate.doubleSpinBoxMaxManip->value());
}
+
+ VR_INFO << "min " << minB[0] << " max " << maxB[0];
+
reachSpace->initialize(currentRobotNodeSet, discrTr, discrRo, minB, maxB, staticModel, dynamicModel, baseNode, tcpNode); //200.0f,0.4f,minB,maxB,staticModel,dynamicModel,baseNode);
if (measure == "Ext. Manipulability")
@@ -607,6 +621,11 @@ void reachabilityWindow::createReach()
}
}
+
+ VR_INFO << "Adding random TCP poses for initialization...";
+ reachSpace->addRandomTCPPoses(12'000'000);
+ VR_INFO << "... done.";
+
reachSpace->print();
reachSpace->addCurrentTCPPose();
--
GitLab
From 8508cb51c1520daedf0986d1f387768f99ad564f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 6 Jul 2021 08:10:40 +0200
Subject: [PATCH 03/14] adding natural posture (cmake)
---
VirtualRobot/CMakeLists.txt | 2 +
VirtualRobot/Workspace/NaturalPosture.cpp | 59 ++++++++++++++++++++---
2 files changed, 55 insertions(+), 6 deletions(-)
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index 8d9a4db46..dacef81f8 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -343,6 +343,7 @@ SET(SOURCES
Workspace/Manipulability.cpp
Workspace/Reachability.cpp
+ Workspace/NaturalPosture.cpp
Workspace/WorkspaceDataArray.cpp
Workspace/WorkspaceGrid.cpp
Workspace/WorkspaceRepresentation.cpp
@@ -584,6 +585,7 @@ SET(INCLUDES
Workspace/Manipulability.h
Workspace/Reachability.h
+ Workspace/NaturalPosture.h
Workspace/VoxelTree6D.hpp
Workspace/VoxelTree6DElement.hpp
Workspace/VoxelTreeND.hpp
diff --git a/VirtualRobot/Workspace/NaturalPosture.cpp b/VirtualRobot/Workspace/NaturalPosture.cpp
index e5a99a594..09e031968 100644
--- a/VirtualRobot/Workspace/NaturalPosture.cpp
+++ b/VirtualRobot/Workspace/NaturalPosture.cpp
@@ -5,6 +5,7 @@
#include <cmath>
#include <fstream>
#include <stdexcept>
+#include <Eigen/src/Core/Matrix.h>
#include <VirtualRobot/IK/CompositeDiffIK/Soechting.h>
#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
@@ -17,6 +18,7 @@
#include "../VirtualRobotException.h"
#include "IK/CompositeDiffIK/CompositeDiffIK.h"
#include "IK/CompositeDiffIK/SoechtingNullspaceGradient.h"
+#include "VirtualRobot.h"
namespace VirtualRobot
{
@@ -30,6 +32,7 @@ namespace VirtualRobot
{
// auto target1 = ik->addTarget(robot->getRobotNode(tcpNode->getName()), VirtualRobot::IKSolver::All);
}
+
float NaturalPosture::evaluate()
{
@@ -77,12 +80,30 @@ namespace VirtualRobot
}
const auto weightedDiff = soechtingNullspaceGradient->getGradient(params, -1);
+ const float e1 = weightedDiff.squaredNorm();
+
+
+
+ auto wristAdduction = nodeSet->getNode("ArmR7_Wri1");
+ auto wristExtension = nodeSet->getNode("ArmR8_Wri2");
+
+ const Eigen::Vector2f wristTarget{ M_PI_2f32 + MathTools::deg2rad(10), M_PI_2f32 - MathTools::deg2rad(20)};
+ const Eigen::Vector2f wristJointValues{wristAdduction->getJointValue(), wristExtension->getJointValue() };
+
+ // set(wristAdduction, 1.F, M_PI_2f32 + MathTools::deg2rad(10));
+ // set(wristExtension, 1.F, M_PI_2f32 - MathTools::deg2rad(20));
+
+ const float e2 = (wristTarget - wristJointValues).squaredNorm();
+
+ return std::sqrt(e1 + e2);
+
- return weightedDiff.norm();
}
void NaturalPosture::addPose(const Eigen::Matrix4f& pose)
{
+ // VR_INFO << "Adding pose";
+
Eigen::Matrix4f p = pose;
toLocal(p);
@@ -137,18 +158,44 @@ namespace VirtualRobot
//cout<<"mSc = "<<mSc<<endl;
//cout<<"e = "<<int(e)<<endl;
- const float e = evaluate();
+ const float ee = evaluate();
+
+ // VR_INFO << "evaluate: " << ee;
+
+
+
+ const float mSc = std::min(ee / 10, 1.0F);
- // add at least 1, since the pose is reachable
- if (e == 0)
+ if(mSc > 1.0)
{
- e = 1;
+ VR_WARNING << "mSc too large " << mSc;
}
- if (e > data->get(v))
+ // // add at least 1, since the pose is reachable
+ // if (e == 0)
+ // {
+ // e = 1;
+ // }
+
+ const auto e = static_cast<unsigned char>(mSc * static_cast<float>(UCHAR_MAX) + 0.5F);
+
+ const auto oldVal = data->get(v);
+ if (oldVal == 0) // unset
{
data->setDatum(v, e);
}
+ else
+ {
+ if(e < oldVal)
+ {
+ data->setDatum(v, e);
+ }
+ }
+
+
+
+ }else {
+ VR_WARNING << "Could not get voxel from pose!";
}
buildUpLoops++;
--
GitLab
From 3f9ce14eed1ec63a71defab808c2f79ac275f7fd Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 6 Jul 2021 14:59:45 +0200
Subject: [PATCH 04/14] less verbose
---
VirtualRobot/Workspace/NaturalPosture.cpp | 4 ++--
VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp | 4 ++--
2 files changed, 4 insertions(+), 4 deletions(-)
diff --git a/VirtualRobot/Workspace/NaturalPosture.cpp b/VirtualRobot/Workspace/NaturalPosture.cpp
index 09e031968..68b96c65e 100644
--- a/VirtualRobot/Workspace/NaturalPosture.cpp
+++ b/VirtualRobot/Workspace/NaturalPosture.cpp
@@ -45,7 +45,7 @@ namespace VirtualRobot
if (robot->getName() == "Armar6" && nodeSet->getName() == "RightArm")
{
- std::cout << "Adding soechting nullspace" << std::endl;
+ // std::cout << "Adding soechting nullspace" << std::endl;
VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
armjoints.clavicula = robot->getRobotNode("ArmR1_Cla1");
armjoints.shoulder1 = robot->getRobotNode("ArmR2_Sho1");
@@ -60,7 +60,7 @@ namespace VirtualRobot
}
else if (robot->getName() == "Armar6" && nodeSet->getName() == "LeftArm")
{
- std::cout << "Adding soechting nullspace" << std::endl;
+ // std::cout << "Adding soechting nullspace" << std::endl;
VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
armjoints.clavicula = robot->getRobotNode("ArmL1_Cla1");
armjoints.shoulder1 = robot->getRobotNode("ArmL2_Sho1");
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
index 2fa4a4316..8822dc756 100644
--- a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
@@ -471,7 +471,7 @@ void DiffIKWidget::solveIK(bool untilReached) {
float kSoechting = ui->kSoechting->value();
if (ui->checkBoxSoechtingNullspace->isChecked() && kSoechting > 0) {
if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "RightArm") {
- std::cout << "Adding soechting nullspace" << std::endl;
+ // std::cout << "Adding soechting nullspace" << std::endl;
VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
armjoints.clavicula = clonedRobot->getRobotNode("ArmR1_Cla1");
armjoints.shoulder1 = clonedRobot->getRobotNode("ArmR2_Sho1");
@@ -484,7 +484,7 @@ void DiffIKWidget::solveIK(bool untilReached) {
ik->addNullspaceGradient(gradient);
}
else if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "LeftArm") {
- std::cout << "Adding soechting nullspace" << std::endl;
+ // std::cout << "Adding soechting nullspace" << std::endl;
VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
armjoints.clavicula = clonedRobot->getRobotNode("ArmL1_Cla1");
armjoints.shoulder1 = clonedRobot->getRobotNode("ArmL2_Sho1");
--
GitLab
From 9e6a45e2588a826498fd1cd829fb9a6e4a75fa43 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Tue, 6 Jul 2021 15:00:01 +0200
Subject: [PATCH 05/14] workspace representation: fix: preventing segfault
---
.../Workspace/WorkspaceRepresentation.cpp | 847 ++++++++++--------
1 file changed, 480 insertions(+), 367 deletions(-)
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index 47b776ed7..750a1ac4b 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -1,24 +1,28 @@
#include "WorkspaceRepresentation.h"
-#include "../VirtualRobotException.h"
+
+#include <cfloat>
+#include <climits>
+#include <cmath>
+#include <fstream>
+#include <mutex>
+#include <thread>
+
+#include <VirtualRobot/Random.h>
+
+#include "../CollisionDetection/CollisionChecker.h"
+#include "../Compression/CompressionBZip2.h"
+#include "../Compression/CompressionRLE.h"
+#include "../Grasping/Grasp.h"
+#include "../Grasping/GraspSet.h"
+#include "../ManipulationObject.h"
+#include "../Nodes/RobotNode.h"
#include "../Robot.h"
#include "../RobotNodeSet.h"
-#include "../Compression/CompressionRLE.h"
-#include "../Compression/CompressionBZip2.h"
#include "../SceneObjectSet.h"
-#include "../Nodes/RobotNode.h"
+#include "../VirtualRobotException.h"
+#include "../Visualization/ColorMap.h"
#include "../Visualization/Visualization.h"
#include "../Visualization/VisualizationFactory.h"
-#include "../CollisionDetection/CollisionChecker.h"
-#include "../Visualization/ColorMap.h"
-#include "../ManipulationObject.h"
-#include "../Grasping/Grasp.h"
-#include "../Grasping/GraspSet.h"
-#include <VirtualRobot/Random.h>
-#include <fstream>
-#include <cmath>
-#include <cfloat>
-#include <climits>
-#include <thread>
namespace VirtualRobot
{
@@ -28,11 +32,11 @@ namespace VirtualRobot
WorkspaceRepresentation::WorkspaceRepresentation(RobotPtr robot)
{
THROW_VR_EXCEPTION_IF(!robot, "Need a robot ptr here");
- this->robot = robot;
- type = "WorkspaceRepresentation";
- versionMajor = 2;
- versionMinor = 9;
- orientationType = Hopf;//EulerXYZExtrinsic;
+ this->robot = robot;
+ type = "WorkspaceRepresentation";
+ versionMajor = 2;
+ versionMinor = 9;
+ orientationType = Hopf; //EulerXYZExtrinsic;
reset();
}
@@ -59,7 +63,6 @@ namespace VirtualRobot
return res;
}
-
int WorkspaceRepresentation::avgAngleReachabilities(int x0, int x1, int x2) const
{
int res = 0;
@@ -80,13 +83,14 @@ namespace VirtualRobot
}
}
- res /= numVoxels[3]* numVoxels[4]* numVoxels[5];
+ res /= numVoxels[3] * numVoxels[4] * numVoxels[5];
return res;
}
-
- void WorkspaceRepresentation::uncompressData(const unsigned char* source, int size, unsigned char* dest)
+ void WorkspaceRepresentation::uncompressData(const unsigned char* source,
+ int size,
+ unsigned char* dest)
{
unsigned char count;
unsigned char value;
@@ -102,7 +106,9 @@ namespace VirtualRobot
}
}
- unsigned char* WorkspaceRepresentation::compressData(const unsigned char* source, int size, int& compressedSize)
+ unsigned char* WorkspaceRepresentation::compressData(const unsigned char* source,
+ int size,
+ int& compressedSize)
{
// on large arrays sometimes an out-of-memory exception is thrown, so in order to reduce the size of the array, we assume we can compress it
// hence, we have to check if the compressed size does not exceed the original size on every pos increase
@@ -110,23 +116,28 @@ namespace VirtualRobot
try
{
- dest = new unsigned char[/*2 * */size];
+ dest = new unsigned char[/*2 * */ size];
}
- catch(const std::exception &e)
+ catch (const std::exception& e)
{
- VR_ERROR << "Error:" << e.what() << endl << "Could not assign " << size << " bytes of memory. Reduce size of WorkspaceRepresentation data..." << std::endl;
+ VR_ERROR << "Error:" << e.what() << endl
+ << "Could not assign " << size
+ << " bytes of memory. Reduce size of WorkspaceRepresentation data..."
+ << std::endl;
throw;
}
catch (...)
{
- VR_ERROR << "Could not assign " << size << " bytes of memory. Reduce size of WorkspaceRepresentation data..." << std::endl;
+ VR_ERROR << "Could not assign " << size
+ << " bytes of memory. Reduce size of WorkspaceRepresentation data..."
+ << std::endl;
throw;
}
int pos = 0;
unsigned char curValue = source[0];
- unsigned char count = 1;
+ unsigned char count = 1;
for (int i = 1; i < size; i++)
{
@@ -134,10 +145,12 @@ namespace VirtualRobot
{
if (count == 255)
{
- dest[pos] = 255;
+ dest[pos] = 255;
dest[pos + 1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos >= size, "Could not perform run-length compression. Data is too cluttered!!!");
+ THROW_VR_EXCEPTION_IF(
+ pos >= size,
+ "Could not perform run-length compression. Data is too cluttered!!!");
count = 1;
}
@@ -148,22 +161,25 @@ namespace VirtualRobot
}
else
{
- dest[pos] = count;
+ dest[pos] = count;
dest[pos + 1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos >= size, "Could not perform run-length compression. Data is too cluttered!!!");
+ THROW_VR_EXCEPTION_IF(
+ pos >= size,
+ "Could not perform run-length compression. Data is too cluttered!!!");
curValue = source[i];
- count = 1;
+ count = 1;
}
}
if (count > 0)
{
- dest[pos] = count;
+ dest[pos] = count;
dest[pos + 1] = curValue;
pos += 2;
- THROW_VR_EXCEPTION_IF(pos >= size, "Could not perform run-length compression. Data is too cluttered!!!");
+ THROW_VR_EXCEPTION_IF(
+ pos >= size, "Could not perform run-length compression. Data is too cluttered!!!");
}
compressedSize = pos;
@@ -191,13 +207,11 @@ namespace VirtualRobot
tmpString == "Reachability Binary File" ||
tmpString == "Reachbaility Binary File" || // typo in old versions
tmpString == "Manipulability Binary File" ||
- tmpString == "ReachabilitySpace Binary File" ||
- tmpString == tmpStr2)
+ tmpString == "ReachabilitySpace Binary File" || tmpString == tmpStr2)
{
fileTypeOK = true;
}
-
THROW_VR_EXCEPTION_IF(!fileTypeOK, "Wrong file format:" << tmpString);
// Check version
@@ -212,9 +226,10 @@ namespace VirtualRobot
// now check if an older version is used
THROW_VR_EXCEPTION_IF(
(version[0] > 2) ||
- (version[0] == 2 && !(version[1]>=0 && version[1] <= 9)) ||
- (version[0] == 1 && !(version[1] == 0 || version[1] == 2 || version[1] == 3)
- ), "Wrong file format version");
+ (version[0] == 2 && !(version[1] >= 0 && version[1] <= 9)) ||
+ (version[0] == 1 &&
+ !(version[1] == 0 || version[1] == 2 || version[1] == 3)),
+ "Wrong file format version");
}
if (version[0] > 2 || (version[0] == 2 && version[1] > 7))
{
@@ -248,21 +263,27 @@ namespace VirtualRobot
{
int nrNodes = (int)(FileIO::read<ioIntTypeRead>(file));
- THROW_VR_EXCEPTION_IF(nrNodes >= 0 && nodeSet->getSize() != static_cast<std::size_t>(nrNodes), "Node Sets don't match (size differs).");
+ THROW_VR_EXCEPTION_IF(nrNodes >= 0 &&
+ nodeSet->getSize() != static_cast<std::size_t>(nrNodes),
+ "Node Sets don't match (size differs).");
// Check joint limits
std::vector<RobotNodePtr> nodes = nodeSet->getAllRobotNodes();
- for (auto & node : nodes)
+ for (auto& node : nodes)
{
float limits[2];
FileIO::readArray<float>(limits, 2, file);
//limits[0] = (int)(FileIO::read<ioIntTypeRead>(file));
//limits[1] = (int)(FileIO::read<ioIntTypeRead>(file));
- if (fabs(node->getJointLimitLo() - limits[0]) > 0.01 || fabs(node->getJointLimitHi() - limits[1]) > 0.01)
+ if (fabs(node->getJointLimitLo() - limits[0]) > 0.01 ||
+ fabs(node->getJointLimitHi() - limits[1]) > 0.01)
{
- VR_WARNING << "Joint limit mismatch for " << node->getName() << ", min: " << node->getJointLimitLo() << " / " << limits[0] << ", max: " << node->getJointLimitHi() << " / " << limits[1] << std::endl;
+ VR_WARNING << "Joint limit mismatch for " << node->getName()
+ << ", min: " << node->getJointLimitLo() << " / " << limits[0]
+ << ", max: " << node->getJointLimitHi() << " / " << limits[1]
+ << std::endl;
}
}
}
@@ -273,7 +294,7 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(!tcpNode, "Unknown TCP");
// Check Base Joint
- if (version[0] > 1 || (version[0] == 1 && version[1] > 0))
+ if (version[0] > 1 || (version[0] == 1 && version[1] > 0))
{
FileIO::readString(tmpString, file);
if (tmpString.empty() || !robot->hasRobotNode(tmpString))
@@ -287,7 +308,6 @@ namespace VirtualRobot
}
}
-
// Static collision model
FileIO::readString(tmpString, file);
@@ -306,18 +326,18 @@ namespace VirtualRobot
buildUpLoops = (int)(FileIO::read<ioIntTypeRead>(file));
- collisionConfigs = (int)(FileIO::read<ioIntTypeRead>(file));
+ collisionConfigs = (int)(FileIO::read<ioIntTypeRead>(file));
discretizeStepTranslation = FileIO::read<float>(file);
- discretizeStepRotation = FileIO::read<float>(file);
+ discretizeStepRotation = FileIO::read<float>(file);
- for (int & numVoxel : numVoxels)
+ for (int& numVoxel : numVoxels)
{
numVoxel = (int)(FileIO::read<ioIntTypeRead>(file));
}
//FileIO::readArray<int>(numVoxels, 6, file);
int voxelFilledCount = (int)(FileIO::read<ioIntTypeRead>(file));
- int maxEntry = (int)(FileIO::read<ioIntTypeRead>(file));
+ int maxEntry = (int)(FileIO::read<ioIntTypeRead>(file));
for (int i = 0; i < 6; i++)
{
@@ -342,10 +362,18 @@ namespace VirtualRobot
// Read Data
FileIO::readString(tmpString, file);
- THROW_VR_EXCEPTION_IF(tmpString != "DATA_START", "Bad file format, expecting DATA_START.");
-
- long size = numVoxels[0] * numVoxels[1] * numVoxels[2] * numVoxels[3] * numVoxels[4] * numVoxels[5];
- data.reset(new WorkspaceDataArray(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5], true));
+ THROW_VR_EXCEPTION_IF(tmpString != "DATA_START",
+ "Bad file format, expecting DATA_START.");
+
+ long size = numVoxels[0] * numVoxels[1] * numVoxels[2] * numVoxels[3] * numVoxels[4] *
+ numVoxels[5];
+ data.reset(new WorkspaceDataArray(numVoxels[0],
+ numVoxels[1],
+ numVoxels[2],
+ numVoxels[3],
+ numVoxels[4],
+ numVoxels[5],
+ true));
if (version[0] <= 1 || (version[0] == 2 && version[1] <= 3))
{
@@ -360,7 +388,7 @@ namespace VirtualRobot
else
{
// Data is compressed
- int compressedSize = (int)(FileIO::read<ioIntTypeRead>(file));
+ int compressedSize = (int)(FileIO::read<ioIntTypeRead>(file));
unsigned char* compressedData = new unsigned char[compressedSize];
FileIO::readArray<unsigned char>(compressedData, compressedSize, file);
@@ -378,12 +406,13 @@ namespace VirtualRobot
// convert old data format
unsigned char* dRot;
- unsigned int sizeX0 = numVoxels[1] * numVoxels[2] * numVoxels[3] * numVoxels[4] * numVoxels[5];
+ unsigned int sizeX0 =
+ numVoxels[1] * numVoxels[2] * numVoxels[3] * numVoxels[4] * numVoxels[5];
unsigned int sizeX1 = numVoxels[2] * numVoxels[3] * numVoxels[4] * numVoxels[5];
unsigned int sizeX2 = numVoxels[3] * numVoxels[4] * numVoxels[5];
unsigned int sizeX3 = numVoxels[4] * numVoxels[5];
unsigned int sizeX4 = numVoxels[5];
- dRot = new unsigned char[numVoxels[3]*numVoxels[4]*numVoxels[5]];
+ dRot = new unsigned char[numVoxels[3] * numVoxels[4] * numVoxels[5]];
for (int x = 0; x < numVoxels[0]; x++)
for (int y = 0; y < numVoxels[1]; y++)
@@ -394,13 +423,14 @@ namespace VirtualRobot
for (int c = 0; c < numVoxels[5]; c++)
{
dRot[a * sizeX3 + b * sizeX4 + c] =
- d[x * sizeX0 + y * sizeX1 + z * sizeX2 + a * sizeX3 + b * sizeX4 + c];
+ d[x * sizeX0 + y * sizeX1 + z * sizeX2 + a * sizeX3 +
+ b * sizeX4 + c];
}
data->setDataRot(dRot, x, y, z);
}
- delete [] dRot;
+ delete[] dRot;
delete[] d;
}
else
@@ -417,7 +447,7 @@ namespace VirtualRobot
if (compressionBZIP2)
{
- int dataSize = numVoxels[3] * numVoxels[4] * numVoxels[5];
+ int dataSize = numVoxels[3] * numVoxels[4] * numVoxels[5];
unsigned char* uncompressedData = new unsigned char[dataSize];
CompressionBZip2Ptr bzip2(new CompressionBZip2(&file));
@@ -464,9 +494,10 @@ namespace VirtualRobot
}
else
{
- int maxCompressedSize = numVoxels[3] * numVoxels[4] * numVoxels[5] * 3;
+ int maxCompressedSize = numVoxels[3] * numVoxels[4] * numVoxels[5] * 3;
unsigned char* compressedData = new unsigned char[maxCompressedSize];
- unsigned char* uncompressedData = new unsigned char[numVoxels[3]*numVoxels[4]*numVoxels[5]];
+ unsigned char* uncompressedData =
+ new unsigned char[numVoxels[3] * numVoxels[4] * numVoxels[5]];
for (int x = 0; x < numVoxels[0]; x++)
for (int y = 0; y < numVoxels[1]; y++)
@@ -474,9 +505,11 @@ namespace VirtualRobot
{
int compressedSize = (int)(FileIO::read<ioIntTypeRead>(file));
- FileIO::readArray<unsigned char>(compressedData, compressedSize, file);
+ FileIO::readArray<unsigned char>(
+ compressedData, compressedSize, file);
- CompressionRLE::RLE_Uncompress(compressedData, uncompressedData, compressedSize);
+ CompressionRLE::RLE_Uncompress(
+ compressedData, uncompressedData, compressedSize);
data->setDataRot(uncompressedData, x, y, z);
}
@@ -485,7 +518,6 @@ namespace VirtualRobot
}
}
-
data->setVoxelFilledCount(voxelFilledCount);
data->setMaxEntry(maxEntry);
@@ -531,7 +563,7 @@ namespace VirtualRobot
const std::vector<RobotNodePtr> nodes = nodeSet->getAllRobotNodes();
FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)(nodes.size()));
- for (const auto & node : nodes)
+ for (const auto& node : nodes)
{
FileIO::write<float>(file, node->getJointLimitLo());
FileIO::write<float>(file, node->getJointLimitHi());
@@ -578,7 +610,7 @@ namespace VirtualRobot
// Number of voxels
//FileIO::writeArray<ioIntTypeWrite>(file, numVoxels, 6);
- for (int & numVoxel : numVoxels)
+ for (int& numVoxel : numVoxels)
{
FileIO::write<ioIntTypeWrite>(file, (ioIntTypeWrite)numVoxel);
}
@@ -676,7 +708,6 @@ namespace VirtualRobot
}
return maxValue;
-
}
float WorkspaceRepresentation::getVoxelSize(int dim) const
@@ -755,7 +786,6 @@ namespace VirtualRobot
positionGlobal = t.block(0, 3, 3, 1);
}
-
void WorkspaceRepresentation::toGlobalVec(Eigen::Vector3f& positionLocal) const
{
Eigen::Matrix4f t;
@@ -767,7 +797,8 @@ namespace VirtualRobot
void WorkspaceRepresentation::setCurrentTCPPoseEntryIfLower(unsigned char e)
{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No WorkspaceRepresentation data loaded");
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode,
+ "No WorkspaceRepresentation data loaded");
Eigen::Matrix4f p = tcpNode->getGlobalPose();
toLocal(p);
@@ -795,7 +826,6 @@ namespace VirtualRobot
data->setDatum(x, e, this);
}
-
buildUpLoops++;
}
@@ -807,23 +837,23 @@ namespace VirtualRobot
for (int i = 0; i < 6; i++)
{
pos = ((x[i] - minBounds[i]) / spaceSize[i]) * (float)numVoxels[i];
- a = (int)(pos);
+ a = (int)(pos);
if (a < 0)
{
// check for rounding errors
- if (a==-1 && (fabs(float(a) - pos)<0.5f))
+ if (a == -1 && (fabs(float(a) - pos) < 0.5f))
a = 0;
else
- return false; //pos[i] = 0; // if pose is outside of voxel space, ignore it
+ return false; //pos[i] = 0; // if pose is outside of voxel space, ignore it
}
else if (a >= numVoxels[i])
{
// check for rounding errors
- if (a==numVoxels[i] && (fabs(float(a) - pos)<0.5f))
- a = numVoxels[i]-1;
+ if (a == numVoxels[i] && (fabs(float(a) - pos) < 0.5f))
+ a = numVoxels[i] - 1;
else
- return false; //pos[i] = m_nVoxels[i]-1; // if pose is outside of voxel space, ignore it
+ return false; //pos[i] = m_nVoxels[i]-1; // if pose is outside of voxel space, ignore it
}
v[i] = a;
@@ -832,7 +862,8 @@ namespace VirtualRobot
return true;
}
- bool WorkspaceRepresentation::getVoxelFromPose(const Eigen::Matrix4f& globalPose, unsigned int v[6]) const
+ bool WorkspaceRepresentation::getVoxelFromPose(const Eigen::Matrix4f& globalPose,
+ unsigned int v[6]) const
{
float x[6];
Eigen::Matrix4f p = globalPose;
@@ -841,8 +872,6 @@ namespace VirtualRobot
return getVoxelFromPose(x, v);
}
-
-
bool WorkspaceRepresentation::getVoxelFromPosition(float x[3], unsigned int v[3]) const
{
int a;
@@ -852,11 +881,11 @@ namespace VirtualRobot
a = (int)(((x[i] - minBounds[i]) / spaceSize[i]) * (float)numVoxels[i]);
if (a < 0)
{
- return false; //pos[i] = 0; // if pose is outside of voxel space, ignore it
+ return false; //pos[i] = 0; // if pose is outside of voxel space, ignore it
}
else if (a >= numVoxels[i])
{
- return false; //pos[i] = m_nVoxels[i]-1; // if pose is outside of voxel space, ignore it
+ return false; //pos[i] = m_nVoxels[i]-1; // if pose is outside of voxel space, ignore it
}
v[i] = a;
@@ -865,7 +894,8 @@ namespace VirtualRobot
return true;
}
- bool WorkspaceRepresentation::getVoxelFromPosition(const Eigen::Matrix4f &globalPose, unsigned int v[3]) const
+ bool WorkspaceRepresentation::getVoxelFromPosition(const Eigen::Matrix4f& globalPose,
+ unsigned int v[3]) const
{
float x[6];
Eigen::Matrix4f p = globalPose;
@@ -878,8 +908,9 @@ namespace VirtualRobot
return getVoxelFromPosition(x2, v);
}
-
- bool WorkspaceRepresentation::setRobotNodesToRandomConfig(VirtualRobot::RobotNodeSetPtr nodeSet, bool checkForSelfCollisions /*= true*/)
+ bool
+ WorkspaceRepresentation::setRobotNodesToRandomConfig(VirtualRobot::RobotNodeSetPtr nodeSet,
+ bool checkForSelfCollisions /*= true*/)
{
if (!nodeSet)
{
@@ -903,9 +934,9 @@ namespace VirtualRobot
for (unsigned int i = 0; i < nodeSet->getSize(); i++)
{
rndValue = RandomFloat(); // value from 0 to 1
- minJ = (*nodeSet)[i]->getJointLimitLo();
- maxJ = (*nodeSet)[i]->getJointLimitHi();
- v[i] = minJ + ((maxJ - minJ) * rndValue);
+ minJ = (*nodeSet)[i]->getJointLimitLo();
+ maxJ = (*nodeSet)[i]->getJointLimitHi();
+ v[i] = minJ + ((maxJ - minJ) * rndValue);
}
robot->setJointValues(nodeSet, v);
@@ -916,26 +947,25 @@ namespace VirtualRobot
return true;
}
- if (!robot->getCollisionChecker()->checkCollision(staticCollisionModel, dynamicCollisionModel))
+ if (!robot->getCollisionChecker()->checkCollision(staticCollisionModel,
+ dynamicCollisionModel))
{
return true;
}
collisionConfigs++;
loop++;
- }
- while (loop < maxLoops);
+ } while (loop < maxLoops);
return false;
}
-
- void WorkspaceRepresentation::addPose(const Eigen::Matrix4f& globalPose, PoseQualityMeasurementPtr /*qualMeasure*/)
+ void WorkspaceRepresentation::addPose(const Eigen::Matrix4f& globalPose,
+ PoseQualityMeasurementPtr /*qualMeasure*/)
{
- addPose(globalPose);
+ addPose(globalPose);
}
-
void WorkspaceRepresentation::addPose(const Eigen::Matrix4f& globalPose)
{
VR_INFO << "oops";
@@ -962,14 +992,11 @@ namespace VirtualRobot
}
}
-
-
data->increaseDatum(x, this);
buildUpLoops++;
}
-
void WorkspaceRepresentation::print()
{
std::cout << "-----------------------------------------------------------" << std::endl;
@@ -1008,24 +1035,24 @@ namespace VirtualRobot
switch (orientationType)
{
- case RPY:
- std::cout << "RPY" << std::endl;
- break;
+ case RPY:
+ std::cout << "RPY" << std::endl;
+ break;
- case EulerXYZ:
- std::cout << "EulerXYZ-Intrinsic" << std::endl;
- break;
+ case EulerXYZ:
+ std::cout << "EulerXYZ-Intrinsic" << std::endl;
+ break;
- case EulerXYZExtrinsic:
- std::cout << "EulerXYZ-Extrinsic" << std::endl;
- break;
+ case EulerXYZExtrinsic:
+ std::cout << "EulerXYZ-Extrinsic" << std::endl;
+ break;
- case Hopf:
- std::cout << "Hopf" << std::endl;
- break;
+ case Hopf:
+ std::cout << "Hopf" << std::endl;
+ break;
- default:
- std::cout << "NYI" << std::endl;
+ default:
+ std::cout << "NYI" << std::endl;
}
std::cout << "CollisionModel static: ";
@@ -1050,11 +1077,18 @@ namespace VirtualRobot
std::cout << "<not set>" << std::endl;
}
- std::cout << "Used " << buildUpLoops << " loops for building the random configs " << std::endl;
- std::cout << "Discretization step sizes: Translation: " << discretizeStepTranslation << " - Rotation: " << discretizeStepRotation << std::endl;
- std::cout << type << " data extends: " << numVoxels[0] << "x" << numVoxels[1] << "x" << numVoxels[2] << "x" << numVoxels[3] << "x" << numVoxels[4] << "x" << numVoxels[5] << std::endl;
- long long nv = (long long)numVoxels[0] * (long long)numVoxels[1] * (long long)numVoxels[2] * (long long)numVoxels[3] * (long long)numVoxels[4] * (long long)numVoxels[5];
- std::cout << "Filled " << data->getVoxelFilledCount() << " of " << nv << " voxels" << std::endl;
+ std::cout << "Used " << buildUpLoops << " loops for building the random configs "
+ << std::endl;
+ std::cout << "Discretization step sizes: Translation: " << discretizeStepTranslation
+ << " - Rotation: " << discretizeStepRotation << std::endl;
+ std::cout << type << " data extends: " << numVoxels[0] << "x" << numVoxels[1] << "x"
+ << numVoxels[2] << "x" << numVoxels[3] << "x" << numVoxels[4] << "x"
+ << numVoxels[5] << std::endl;
+ long long nv = (long long)numVoxels[0] * (long long)numVoxels[1] *
+ (long long)numVoxels[2] * (long long)numVoxels[3] *
+ (long long)numVoxels[4] * (long long)numVoxels[5];
+ std::cout << "Filled " << data->getVoxelFilledCount() << " of " << nv << " voxels"
+ << std::endl;
std::cout << "Collisions: " << collisionConfigs << std::endl;
std::cout << "Maximum entry in a voxel: " << (int)data->getMaxEntry() << std::endl;
std::cout << type << " workspace extend (as defined on construction):" << std::endl;
@@ -1110,32 +1144,36 @@ namespace VirtualRobot
baseNode.reset();
staticCollisionModel.reset();
dynamicCollisionModel.reset();
- buildUpLoops = 0;
- collisionConfigs = 0;
+ buildUpLoops = 0;
+ collisionConfigs = 0;
discretizeStepTranslation = 0;
- discretizeStepRotation = 0;
+ discretizeStepRotation = 0;
for (int i = 0; i < 6; i++)
{
- minBounds[i] = FLT_MAX;
- maxBounds[i] = -FLT_MAX;
+ minBounds[i] = FLT_MAX;
+ maxBounds[i] = -FLT_MAX;
achievedMinValues[i] = FLT_MAX;
achievedMaxValues[i] = -FLT_MAX;
- numVoxels[i] = 0;
- spaceSize[i] = 0;
+ numVoxels[i] = 0;
+ spaceSize[i] = 0;
}
}
- void WorkspaceRepresentation::initialize(RobotNodeSetPtr nodeSet, float discretizeStepTranslation, float discretizeStepRotation,
- float minBounds[6], float maxBounds[6],
- SceneObjectSetPtr staticCollisionModel,
- SceneObjectSetPtr dynamicCollisionModel,
- RobotNodePtr baseNode /*= RobotNodePtr()*/,
- RobotNodePtr tcpNode /*= RobotNodePtr()*/,
- bool adjustOnOverflow /* = true */)
+ void WorkspaceRepresentation::initialize(RobotNodeSetPtr nodeSet,
+ float discretizeStepTranslation,
+ float discretizeStepRotation,
+ float minBounds[6],
+ float maxBounds[6],
+ SceneObjectSetPtr staticCollisionModel,
+ SceneObjectSetPtr dynamicCollisionModel,
+ RobotNodePtr baseNode /*= RobotNodePtr()*/,
+ RobotNodePtr tcpNode /*= RobotNodePtr()*/,
+ bool adjustOnOverflow /* = true */)
{
reset();
- THROW_VR_EXCEPTION_IF((discretizeStepTranslation <= 0.0f || discretizeStepRotation <= 0.0f), "Need positive discretize steps");
+ THROW_VR_EXCEPTION_IF((discretizeStepTranslation <= 0.0f || discretizeStepRotation <= 0.0f),
+ "Need positive discretize steps");
for (int i = 0; i < 6; i++)
{
@@ -1143,7 +1181,8 @@ namespace VirtualRobot
}
THROW_VR_EXCEPTION_IF(!nodeSet, "NULL data, need a nodeSet");
- THROW_VR_EXCEPTION_IF(!nodeSet->isKinematicChain(), "nodeSet must be a valid kinematic chain!");
+ THROW_VR_EXCEPTION_IF(!nodeSet->isKinematicChain(),
+ "nodeSet must be a valid kinematic chain!");
this->nodeSet = nodeSet;
this->tcpNode = nodeSet->getTCP();
@@ -1152,7 +1191,8 @@ namespace VirtualRobot
this->tcpNode = tcpNode;
}
- THROW_VR_EXCEPTION_IF(!robot->hasRobotNode(this->tcpNode), "robot does not know tcp:" << this->tcpNode->getName());
+ THROW_VR_EXCEPTION_IF(!robot->hasRobotNode(this->tcpNode),
+ "robot does not know tcp:" << this->tcpNode->getName());
this->baseNode = baseNode;
if (baseNode && !robot->hasRobotNode(baseNode))
@@ -1160,8 +1200,11 @@ namespace VirtualRobot
THROW_VR_EXCEPTION("Robot does not know basenode:" << baseNode->getName());
}
- THROW_VR_EXCEPTION_IF(nodeSet->hasRobotNode(baseNode), " baseNode is part of RobotNodeSet! This is not a good idea, since the globalPose of the baseNode will change during buildup of WorkspaceRepresentation data...");
- this->staticCollisionModel = staticCollisionModel;
+ THROW_VR_EXCEPTION_IF(
+ nodeSet->hasRobotNode(baseNode),
+ " baseNode is part of RobotNodeSet! This is not a good idea, since the globalPose of "
+ "the baseNode will change during buildup of WorkspaceRepresentation data...");
+ this->staticCollisionModel = staticCollisionModel;
this->dynamicCollisionModel = dynamicCollisionModel;
if (!staticCollisionModel || !dynamicCollisionModel)
@@ -1171,18 +1214,20 @@ namespace VirtualRobot
}
else
{
- THROW_VR_EXCEPTION_IF(staticCollisionModel->getCollisionChecker() != dynamicCollisionModel->getCollisionChecker(), "Need same collision checker instance!");
+ THROW_VR_EXCEPTION_IF(staticCollisionModel->getCollisionChecker() !=
+ dynamicCollisionModel->getCollisionChecker(),
+ "Need same collision checker instance!");
}
// build data
this->discretizeStepTranslation = discretizeStepTranslation;
- this->discretizeStepRotation = discretizeStepRotation;
+ this->discretizeStepRotation = discretizeStepRotation;
for (int i = 0; i < 6; i++)
{
this->minBounds[i] = minBounds[i];
this->maxBounds[i] = maxBounds[i];
- spaceSize[i] = maxBounds[i] - minBounds[i];
+ spaceSize[i] = maxBounds[i] - minBounds[i];
if (i < 3)
{
@@ -1196,7 +1241,13 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF((numVoxels[i] <= 0), " numVoxels <= 0 in dimension " << i);
}
- data.reset(new WorkspaceDataArray(numVoxels[0], numVoxels[1], numVoxels[2], numVoxels[3], numVoxels[4], numVoxels[5], adjustOnOverflow));
+ data.reset(new WorkspaceDataArray(numVoxels[0],
+ numVoxels[1],
+ numVoxels[2],
+ numVoxels[3],
+ numVoxels[4],
+ numVoxels[5],
+ adjustOnOverflow));
customInitialize();
}
@@ -1228,8 +1279,8 @@ namespace VirtualRobot
return data->get(x, this);
}
-
- Eigen::Matrix4f WorkspaceRepresentation::getPoseFromVoxel(unsigned int v[6], bool transformToGlobalPose)
+ Eigen::Matrix4f WorkspaceRepresentation::getPoseFromVoxel(unsigned int v[6],
+ bool transformToGlobalPose)
{
float x[6];
@@ -1250,7 +1301,7 @@ namespace VirtualRobot
return false;
}
- v[i] = ((((float) x[i]) * spaceSize[i]) / ((float)numVoxels[i])) + minBounds[i];
+ v[i] = ((((float)x[i]) * spaceSize[i]) / ((float)numVoxels[i])) + minBounds[i];
if (i < 3)
{
@@ -1268,12 +1319,12 @@ namespace VirtualRobot
WorkspaceRepresentation::VolumeInfo WorkspaceRepresentation::computeVolumeInformation()
{
WorkspaceRepresentation::VolumeInfo result;
- result.borderVoxelCount3D = 0;
- result.filledVoxelCount3D = 0;
- result.volume3D = 0;
+ result.borderVoxelCount3D = 0;
+ result.filledVoxelCount3D = 0;
+ result.volume3D = 0;
result.volumeFilledVoxels3D = 0;
- result.volumeVoxel3D = 0;
- result.voxelCount3D = numVoxels[0]*numVoxels[1]*numVoxels[2];
+ result.volumeVoxel3D = 0;
+ result.voxelCount3D = numVoxels[0] * numVoxels[1] * numVoxels[2];
for (int a = 0; a < numVoxels[0]; a++)
{
for (int b = 0; b < numVoxels[1]; b++)
@@ -1285,40 +1336,44 @@ namespace VirtualRobot
if (value > 0)
{
result.filledVoxelCount3D++;
- if (a==0 || b==0 || c==0 || a==numVoxels[0]-1 || b==numVoxels[1]-1 || c==numVoxels[2]-1)
+ if (a == 0 || b == 0 || c == 0 || a == numVoxels[0] - 1 ||
+ b == numVoxels[1] - 1 || c == numVoxels[2] - 1)
result.borderVoxelCount3D++;
else
{
int neighborEmptyCount = 0;
- if (sumAngleReachabilities(a-1, b, c)==0)
+ if (sumAngleReachabilities(a - 1, b, c) == 0)
neighborEmptyCount++;
- if (sumAngleReachabilities(a+1, b, c)==0)
+ if (sumAngleReachabilities(a + 1, b, c) == 0)
neighborEmptyCount++;
- if (sumAngleReachabilities(a, b-1, c)==0)
+ if (sumAngleReachabilities(a, b - 1, c) == 0)
neighborEmptyCount++;
- if (sumAngleReachabilities(a, b+1, c)==0)
+ if (sumAngleReachabilities(a, b + 1, c) == 0)
neighborEmptyCount++;
- if (sumAngleReachabilities(a, b, c-1)==0)
+ if (sumAngleReachabilities(a, b, c - 1) == 0)
neighborEmptyCount++;
- if (sumAngleReachabilities(a, b, c+1)==0)
+ if (sumAngleReachabilities(a, b, c + 1) == 0)
neighborEmptyCount++;
- if (neighborEmptyCount>=1)
+ if (neighborEmptyCount >= 1)
result.borderVoxelCount3D++;
}
}
}
}
}
- double discrM = discretizeStepTranslation * 0.001;
- double voxelVolume = discrM * discrM * discrM;
- result.volumeVoxel3D = (float)voxelVolume;
+ double discrM = discretizeStepTranslation * 0.001;
+ double voxelVolume = discrM * discrM * discrM;
+ result.volumeVoxel3D = (float)voxelVolume;
result.volumeFilledVoxels3D = (float)((double)result.filledVoxelCount3D * voxelVolume);
- result.volume3D = (float)(((double)result.filledVoxelCount3D - 0.5*(double)result.borderVoxelCount3D) * voxelVolume);
+ result.volume3D =
+ (float)(((double)result.filledVoxelCount3D - 0.5 * (double)result.borderVoxelCount3D) *
+ voxelVolume);
return result;
}
- Eigen::Matrix4f WorkspaceRepresentation::getPoseFromVoxel(float v[6], bool transformToGlobalPose /*= true*/)
+ Eigen::Matrix4f WorkspaceRepresentation::getPoseFromVoxel(float v[6],
+ bool transformToGlobalPose /*= true*/)
{
float x[6];
@@ -1341,8 +1396,8 @@ namespace VirtualRobot
Eigen::Matrix4f WorkspaceRepresentation::sampleCoveredPose()
{
- int maxLoops = 10000;
- int i = 0;
+ int maxLoops = 10000;
+ int i = 0;
Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
unsigned int nV[6];
float x[6];
@@ -1398,10 +1453,10 @@ namespace VirtualRobot
{
if (data->get(x) > 0)
{
- int sum = 0;
+ int sum = 0;
int count = 0;
- for (unsigned int & i : x)
+ for (unsigned int& i : x)
{
i--;
@@ -1429,8 +1484,6 @@ namespace VirtualRobot
sum /= count;
newData->setDatum(x, (unsigned char)sum);
}
-
-
}
}
@@ -1459,10 +1512,16 @@ namespace VirtualRobot
return maxBounds[dim];
}
- unsigned char WorkspaceRepresentation::getVoxelEntry(unsigned int a, unsigned int b, unsigned int c, unsigned int d, unsigned int e, unsigned int f) const
+ unsigned char WorkspaceRepresentation::getVoxelEntry(unsigned int a,
+ unsigned int b,
+ unsigned int c,
+ unsigned int d,
+ unsigned int e,
+ unsigned int f) const
{
if (/*a < 0 || b < 0 || c < 0 || d < 0 || e < 0 || f < 0 ||*/
- int(a) >= numVoxels[0] || int(b) >= numVoxels[1] || int(c) >= numVoxels[2] || int(d) >= numVoxels[3] || int(e) >= numVoxels[4] || int(f) >= numVoxels[5])
+ int(a) >= numVoxels[0] || int(b) >= numVoxels[1] || int(c) >= numVoxels[2] ||
+ int(d) >= numVoxels[3] || int(e) >= numVoxels[4] || int(f) >= numVoxels[5])
{
return 0;
}
@@ -1498,13 +1557,11 @@ namespace VirtualRobot
return (getEntry(globalPose) > 0);
}
-
bool WorkspaceRepresentation::isCovered(unsigned int v[6])
{
return (data->get(v) > 0);
}
-
void WorkspaceRepresentation::setVoxelEntry(unsigned int v[6], unsigned char e)
{
data->setDatum(v, e);
@@ -1513,14 +1570,19 @@ namespace VirtualRobot
void WorkspaceRepresentation::setCurrentTCPPoseEntry(unsigned char e)
{
- THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No WorkspaceRepresentation data loaded");
+ THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode,
+ "No WorkspaceRepresentation data loaded");
Eigen::Matrix4f p = tcpNode->getGlobalPose();
setEntry(p, e);
-
}
- bool WorkspaceRepresentation::checkForParameters(RobotNodeSetPtr nodeSet, float steps, float storeMinBounds[6], float storeMaxBounds[6], RobotNodePtr baseNode, RobotNodePtr tcpNode)
+ bool WorkspaceRepresentation::checkForParameters(RobotNodeSetPtr nodeSet,
+ float steps,
+ float storeMinBounds[6],
+ float storeMaxBounds[6],
+ RobotNodePtr baseNode,
+ RobotNodePtr tcpNode)
{
if (!robot || !nodeSet || !nodeSet->isKinematicChain())
{
@@ -1558,7 +1620,7 @@ namespace VirtualRobot
// toLocal uses this->baseNode!
RobotNodePtr tmpBase = this->baseNode;
- this->baseNode = baseNode;
+ this->baseNode = baseNode;
for (int i = 0; i < steps; i++)
{
@@ -1591,12 +1653,12 @@ namespace VirtualRobot
// assume higher values
for (int i = 0; i < 6; i++)
{
- float sizex = storeMaxBounds[i] - storeMinBounds[i];
+ float sizex = storeMaxBounds[i] - storeMinBounds[i];
float factor = 0.2f;
if (i > 2)
{
- factor = 0.05f; // adjustment for rotation is smaller
+ factor = 0.05f; // adjustment for rotation is smaller
}
storeMinBounds[i] -= sizex * factor;
@@ -1605,10 +1667,12 @@ namespace VirtualRobot
this->baseNode = tmpBase;
return true;
-
}
- WorkspaceRepresentation::WorkspaceCut2DPtr WorkspaceRepresentation::createCut(const Eigen::Matrix4f& referencePose, float cellSize, bool sumAngles) const
+ WorkspaceRepresentation::WorkspaceCut2DPtr
+ WorkspaceRepresentation::createCut(const Eigen::Matrix4f& referencePose,
+ float cellSize,
+ bool sumAngles) const
{
WorkspaceCut2DPtr result(new WorkspaceCut2D());
result->referenceGlobalPose = referencePose;
@@ -1623,12 +1687,11 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(cellSize <= 0.0f, "Invalid parameter");
- float sizeX = result->maxBounds[0] - result->minBounds[0];
+ float sizeX = result->maxBounds[0] - result->minBounds[0];
int numVoxelsX = (int)(sizeX / cellSize);
- float sizeY = result->maxBounds[1] - result->minBounds[1];
+ float sizeY = result->maxBounds[1] - result->minBounds[1];
int numVoxelsY = (int)(sizeY / cellSize);
-
Eigen::Matrix4f tmpPose = referencePose;
Eigen::Matrix4f localPose;
float x[6];
@@ -1636,7 +1699,6 @@ namespace VirtualRobot
result->entries.resize(numVoxelsX, numVoxelsY);
-
for (int a = 0; a < numVoxelsX; a++)
{
tmpPose(0, 3) = result->minBounds[0] + (float)a * cellSize + 0.5f * cellSize;
@@ -1648,14 +1710,16 @@ namespace VirtualRobot
{
localPose = tmpPose;
toLocal(localPose);
- matrix2Vector(localPose,x);
+ matrix2Vector(localPose, x);
if (!getVoxelFromPose(x, v))
{
result->entries(a, b) = 0;
- } else
- result->entries(a, b) = sumAngleReachabilities(v[0],v[1],v[2]);
- } else
+ }
+ else
+ result->entries(a, b) = sumAngleReachabilities(v[0], v[1], v[2]);
+ }
+ else
{
result->entries(a, b) = getEntry(tmpPose);
}
@@ -1665,7 +1729,7 @@ namespace VirtualRobot
return result;
}
- /*
+ /*
WorkspaceRepresentation::WorkspaceCut2DPtr WorkspaceRepresentation::createCut(Eigen::Matrix4f gp, float cellSize) const
{
result->referenceGlobalPose = refPose;
@@ -1698,10 +1762,11 @@ namespace VirtualRobot
}*/
- WorkspaceRepresentation::WorkspaceCut2DPtr WorkspaceRepresentation::createCut(float heightPercent, float cellSize, bool sumAngles) const
+ WorkspaceRepresentation::WorkspaceCut2DPtr
+ WorkspaceRepresentation::createCut(float heightPercent, float cellSize, bool sumAngles) const
{
THROW_VR_EXCEPTION_IF(cellSize <= 0.0f, "Invalid parameter");
- THROW_VR_EXCEPTION_IF(heightPercent < 0.0f || heightPercent>1.0f, "Invalid parameter");
+ THROW_VR_EXCEPTION_IF(heightPercent < 0.0f || heightPercent > 1.0f, "Invalid parameter");
WorkspaceCut2DPtr result(new WorkspaceCut2D());
@@ -1713,23 +1778,24 @@ namespace VirtualRobot
result->minBounds[1] = minBB(1);
result->maxBounds[1] = maxBB(1);
- float sizeX = result->maxBounds[0] - result->minBounds[0];
+ float sizeX = result->maxBounds[0] - result->minBounds[0];
int numVoxelsX = (int)(sizeX / cellSize);
- float sizeY = result->maxBounds[1] - result->minBounds[1];
+ float sizeY = result->maxBounds[1] - result->minBounds[1];
int numVoxelsY = (int)(sizeY / cellSize);
float sizeZGlobal = maxBB(2) - minBB(2);
- float poseZGlobal = minBB(2) + heightPercent*sizeZGlobal;
+ float poseZGlobal = minBB(2) + heightPercent * sizeZGlobal;
result->entries.resize(numVoxelsX, numVoxelsY);
result->entries.setZero();
Eigen::Matrix4f refPose = getToGlobalTransformation();
- refPose(2,3) = poseZGlobal;
+ refPose(2, 3) = poseZGlobal;
return createCut(refPose, cellSize, sumAngles);
}
- bool WorkspaceRepresentation::getWorkspaceExtends(Eigen::Vector3f& storeMinBBox, Eigen::Vector3f& storeMaxBBox) const
+ bool WorkspaceRepresentation::getWorkspaceExtends(Eigen::Vector3f& storeMinBBox,
+ Eigen::Vector3f& storeMaxBBox) const
{
Eigen::Vector3f quadPos[8];
float x, y, z;
@@ -1772,7 +1838,7 @@ namespace VirtualRobot
storeMinBBox = quadPos[0];
storeMaxBBox = quadPos[0];
- for (auto & quadPo : quadPos)
+ for (auto& quadPo : quadPos)
{
for (int i = 0; i < 3; i++)
{
@@ -1791,7 +1857,10 @@ namespace VirtualRobot
return true;
}
- std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr> WorkspaceRepresentation::createCutTransformations(WorkspaceRepresentation::WorkspaceCut2DPtr cutXY, RobotNodePtr referenceNode)
+ std::vector<WorkspaceRepresentation::WorkspaceCut2DTransformationPtr>
+ WorkspaceRepresentation::createCutTransformations(
+ WorkspaceRepresentation::WorkspaceCut2DPtr cutXY,
+ RobotNodePtr referenceNode)
{
THROW_VR_EXCEPTION_IF(!cutXY, "NULL data");
@@ -1816,15 +1885,18 @@ namespace VirtualRobot
{
WorkspaceCut2DTransformationPtr tp(new WorkspaceCut2DTransformation());
tp->value = v;
- float xPos = cutXY->minBounds[0] + (float)x * sizeX + 0.5f * sizeX; // center of voxel
- float yPos = cutXY->minBounds[1] + (float)y * sizeY + 0.5f * sizeY; // center of voxel
- tp->transformation = cutXY->referenceGlobalPose;
+ float xPos =
+ cutXY->minBounds[0] + (float)x * sizeX + 0.5f * sizeX; // center of voxel
+ float yPos =
+ cutXY->minBounds[1] + (float)y * sizeY + 0.5f * sizeY; // center of voxel
+ tp->transformation = cutXY->referenceGlobalPose;
tp->transformation(0, 3) = xPos;
tp->transformation(1, 3) = yPos;
if (referenceNode)
{
- tp->transformation = referenceNode->toLocalCoordinateSystem(tp->transformation);
+ tp->transformation =
+ referenceNode->toLocalCoordinateSystem(tp->transformation);
}
result.push_back(tp);
@@ -1850,7 +1922,6 @@ namespace VirtualRobot
THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "No reachability data loaded");
Eigen::Matrix4f p = tcpNode->getGlobalPose();
- VR_INFO << "Adding pose";
addPose(p);
}
@@ -1859,7 +1930,9 @@ namespace VirtualRobot
setEntryCheckNeighbors(poseGlobal, e, 0);
}
- void WorkspaceRepresentation::setEntryCheckNeighbors(const Eigen::Matrix4f& poseGlobal, unsigned char e, unsigned int neighborVoxels)
+ void WorkspaceRepresentation::setEntryCheckNeighbors(const Eigen::Matrix4f& poseGlobal,
+ unsigned char e,
+ unsigned int neighborVoxels)
{
Eigen::Matrix4f p = poseGlobal;
toLocal(p);
@@ -1892,7 +1965,6 @@ namespace VirtualRobot
}
buildUpLoops++;
-
}
MathTools::OOBB WorkspaceRepresentation::getOOBB(bool achievedValues) const
@@ -1918,7 +1990,7 @@ namespace VirtualRobot
void WorkspaceRepresentation::clear()
{
data->clear();
- buildUpLoops = 0;
+ buildUpLoops = 0;
collisionConfigs = 0;
for (int i = 0; i < 6; i++)
@@ -1937,58 +2009,58 @@ namespace VirtualRobot
{
switch (orientationType)
{
- case EulerXYZ:
- {
- x[0] = m(0, 3);
- x[1] = m(1, 3);
- x[2] = m(2, 3);
+ case EulerXYZ:
+ {
+ x[0] = m(0, 3);
+ x[1] = m(1, 3);
+ x[2] = m(2, 3);
- Eigen::Matrix3f m_3 = m.block(0, 0, 3, 3);
- Eigen::Vector3f rotEulerxyz = m_3.eulerAngles(0, 1, 2);
+ Eigen::Matrix3f m_3 = m.block(0, 0, 3, 3);
+ Eigen::Vector3f rotEulerxyz = m_3.eulerAngles(0, 1, 2);
- // intrinsic rotation (x y z)
- x[3] = rotEulerxyz(0);
- x[4] = rotEulerxyz(1);
- x[5] = rotEulerxyz(2);
- }
- break;
+ // intrinsic rotation (x y z)
+ x[3] = rotEulerxyz(0);
+ x[4] = rotEulerxyz(1);
+ x[5] = rotEulerxyz(2);
+ }
+ break;
- case EulerXYZExtrinsic:
- {
- x[0] = m(0, 3);
- x[1] = m(1, 3);
- x[2] = m(2, 3);
+ case EulerXYZExtrinsic:
+ {
+ x[0] = m(0, 3);
+ x[1] = m(1, 3);
+ x[2] = m(2, 3);
- Eigen::Matrix3f m_3 = m.block(0, 0, 3, 3);
- Eigen::Vector3f rotEulerxyz = m_3.eulerAngles(0, 1, 2);
+ Eigen::Matrix3f m_3 = m.block(0, 0, 3, 3);
+ Eigen::Vector3f rotEulerxyz = m_3.eulerAngles(0, 1, 2);
- // extrinsic (fixed coord system) rotation (x y z)
- x[5] = rotEulerxyz(0);
- x[4] = rotEulerxyz(1);
- x[3] = rotEulerxyz(2);
- }
- break;
+ // extrinsic (fixed coord system) rotation (x y z)
+ x[5] = rotEulerxyz(0);
+ x[4] = rotEulerxyz(1);
+ x[3] = rotEulerxyz(2);
+ }
+ break;
- case RPY:
- {
- MathTools::eigen4f2rpy(m, x);
- }
- break;
- case Hopf:
- {
- MathTools::Quaternion q = MathTools::eigen4f2quat(m);
- Eigen::Vector3f h = MathTools::quat2hopf(q);
- x[0] = m(0,3);
- x[1] = m(1,3);
- x[2] = m(2,3);
- x[3] = h(0);
- x[4] = h(1);
- x[5] = h(2);
- }
- break;
+ case RPY:
+ {
+ MathTools::eigen4f2rpy(m, x);
+ }
+ break;
+ case Hopf:
+ {
+ MathTools::Quaternion q = MathTools::eigen4f2quat(m);
+ Eigen::Vector3f h = MathTools::quat2hopf(q);
+ x[0] = m(0, 3);
+ x[1] = m(1, 3);
+ x[2] = m(2, 3);
+ x[3] = h(0);
+ x[4] = h(1);
+ x[5] = h(2);
+ }
+ break;
- default:
- THROW_VR_EXCEPTION("mode nyi...");
+ default:
+ THROW_VR_EXCEPTION("mode nyi...");
}
}
/*
@@ -2014,20 +2086,20 @@ namespace VirtualRobot
{
switch (orientationType)
{
- case EulerXYZ:
- {
- m.setIdentity();
- m(0, 3) = x[0];
- m(1, 3) = x[1];
- m(2, 3) = x[2];
- Eigen::Matrix3f m_3;
- m_3 = Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitX())
- * Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY())
- * Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitZ());
- /*m_3 = Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitZ())
+ case EulerXYZ:
+ {
+ m.setIdentity();
+ m(0, 3) = x[0];
+ m(1, 3) = x[1];
+ m(2, 3) = x[2];
+ Eigen::Matrix3f m_3;
+ m_3 = Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitZ());
+ /*m_3 = Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitZ())
* Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY())
* Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitX());*/
- /*
+ /*
float s1 = sin(x[3]);float s2 = sin(x[4]);float s3 = sin(x[5]);
float c1 = cos(x[3]);float c2 = cos(x[4]);float c3 = cos(x[5]);
// Euler XYZ
@@ -2035,55 +2107,56 @@ namespace VirtualRobot
m_3(1,0) = c1*s3+c3*s1*s2; m_3(1,1) = c1*c3-s1*s2*s3; m_3(1,2) = -c2*s1;
m_3(2,0) = s1*s3-c1*c3*s2; m_3(2,1) = c3*s1+c1*s2*s3; m_3(2,2) = c1*c2;
*/
- /*
+ /*
// Euler ZYX
m_3(0,0) = c1*c2; m_3(0,1) = c1*s2*s3-c3*s1; m_3(0,2) = s1*s3+c1*c3*s2;
m_3(1,0) = c2*s1; m_3(1,1) = c1*c3+s1*s2*s3; m_3(1,2) = c3*s1*s2-c1*s3;
m_3(2,0) = -s2; m_3(2,1) = c2*s3; m_3(2,2) = c2*c3;
*/
- m.block(0, 0, 3, 3) = m_3;
- }
- break;
-
- case EulerXYZExtrinsic:
- {
- m.setIdentity();
- m(0, 3) = x[0];
- m(1, 3) = x[1];
- m(2, 3) = x[2];
- Eigen::Matrix3f m_3;
- m_3 = Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitX())
- * Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY())
- * Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitZ());
- m.block(0, 0, 3, 3) = m_3;
- }
- break;
+ m.block(0, 0, 3, 3) = m_3;
+ }
+ break;
- case RPY:
- {
- MathTools::posrpy2eigen4f(x, m);
- }
- break;
+ case EulerXYZExtrinsic:
+ {
+ m.setIdentity();
+ m(0, 3) = x[0];
+ m(1, 3) = x[1];
+ m(2, 3) = x[2];
+ Eigen::Matrix3f m_3;
+ m_3 = Eigen::AngleAxisf(x[5], Eigen::Vector3f::UnitX()) *
+ Eigen::AngleAxisf(x[4], Eigen::Vector3f::UnitY()) *
+ Eigen::AngleAxisf(x[3], Eigen::Vector3f::UnitZ());
+ m.block(0, 0, 3, 3) = m_3;
+ }
+ break;
+ case RPY:
+ {
+ MathTools::posrpy2eigen4f(x, m);
+ }
+ break;
- case Hopf:
- {
- Eigen::Vector3f h;
- h << x[3],x[4],x[5];
- MathTools::Quaternion q = MathTools::hopf2quat(h);
- m = MathTools::quat2eigen4f(q);
- m(0, 3) = x[0];
- m(1, 3) = x[1];
- m(2, 3) = x[2];
- }
- break;
- default:
- THROW_VR_EXCEPTION("mode nyi...");
+ case Hopf:
+ {
+ Eigen::Vector3f h;
+ h << x[3], x[4], x[5];
+ MathTools::Quaternion q = MathTools::hopf2quat(h);
+ m = MathTools::quat2eigen4f(q);
+ m(0, 3) = x[0];
+ m(1, 3) = x[1];
+ m(2, 3) = x[2];
+ }
+ break;
+ default:
+ THROW_VR_EXCEPTION("mode nyi...");
}
}
- void WorkspaceRepresentation::vector2Matrix(const Eigen::Vector3f& pos, const Eigen::Vector3f& rot, Eigen::Matrix4f& m) const
+ void WorkspaceRepresentation::vector2Matrix(const Eigen::Vector3f& pos,
+ const Eigen::Vector3f& rot,
+ Eigen::Matrix4f& m) const
{
float x[6];
x[0] = pos[0];
@@ -2106,17 +2179,17 @@ namespace VirtualRobot
res->setOrientationType(this->orientationType);
res->versionMajor = this->versionMajor;
res->versionMinor = this->versionMinor;
- res->nodeSet = this->nodeSet;
- res->type = this->type;
-
- res->baseNode = this->baseNode;
- res->tcpNode = this->tcpNode;
- res->staticCollisionModel = this->staticCollisionModel;
- res->dynamicCollisionModel = this->dynamicCollisionModel;
- res->buildUpLoops = this->buildUpLoops;
- res->collisionConfigs = this->collisionConfigs;
+ res->nodeSet = this->nodeSet;
+ res->type = this->type;
+
+ res->baseNode = this->baseNode;
+ res->tcpNode = this->tcpNode;
+ res->staticCollisionModel = this->staticCollisionModel;
+ res->dynamicCollisionModel = this->dynamicCollisionModel;
+ res->buildUpLoops = this->buildUpLoops;
+ res->collisionConfigs = this->collisionConfigs;
res->discretizeStepTranslation = this->discretizeStepTranslation;
- res->discretizeStepRotation = this->discretizeStepRotation;
+ res->discretizeStepRotation = this->discretizeStepRotation;
memcpy(res->minBounds, this->minBounds, sizeof(float) * 6);
memcpy(res->maxBounds, this->maxBounds, sizeof(float) * 6);
memcpy(res->numVoxels, this->numVoxels, sizeof(float) * 6);
@@ -2139,6 +2212,8 @@ namespace VirtualRobot
{
THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Workspace data not initialized");
+ VR_INFO << "Adding random TCP poses";
+
std::vector<float> c;
nodeSet->getJointValues(c);
bool visuSate = robot->getUpdateVisualizationStatus();
@@ -2160,9 +2235,12 @@ namespace VirtualRobot
nodeSet->setJointValues(c);
}
- void WorkspaceRepresentation::addRandomTCPPoses(unsigned int loops, unsigned int numThreads, bool checkForSelfCollisions)
+ void WorkspaceRepresentation::addRandomTCPPoses(unsigned int loops,
+ unsigned int numThreads,
+ bool checkForSelfCollisions)
{
THROW_VR_EXCEPTION_IF(!data || !nodeSet || !tcpNode, "Workspace data not initialized");
+ VR_WARNING << "one";
if (numThreads > loops)
{
@@ -2173,88 +2251,123 @@ namespace VirtualRobot
std::vector<std::thread> threads(numThreads);
unsigned int numPosesPerThread = loops / numThreads; // todo
+ VR_WARNING << "getUpdateVisualizationStatus";
+
+ const bool visuSate = robot->getUpdateVisualizationStatus();
+ VR_WARNING << "setUpdateVisualization";
+
+ robot->setUpdateVisualization(false);
+
+ VR_WARNING << "Using " << numThreads << " threads to fill grid";
+ std::cout << "Samples per thread: " << numPosesPerThread;
+
+ static std::vector<RobotPtr> robots;
+ std::mutex robotsMtx;
+
for (unsigned int i = 0; i < numThreads; i++)
{
- threads[i] = std::thread([=, this] ()
- {
- // each thread gets a cloned robot
- CollisionCheckerPtr cc(new CollisionChecker());
- RobotPtr clonedRobot = this->robot->clone("clonedRobot_" + std::to_string(i), cc);
- clonedRobot->setUpdateVisualization(false);
- RobotNodeSetPtr clonedNodeSet = clonedRobot->getRobotNodeSet(this->nodeSet->getName());
- RobotNodePtr clonedTcpNode = clonedRobot->getRobotNode(this->tcpNode->getName());
-
- SceneObjectSetPtr static_collision_model = this->staticCollisionModel;
- if (static_collision_model && clonedRobot->hasRobotNodeSet(static_collision_model->getName()))
+ threads[i] = std::thread(
+ [=, &robotsMtx, this]()
{
- static_collision_model = clonedRobot->getRobotNodeSet(static_collision_model->getName());
- }
+ // each thread gets a cloned robot
+ CollisionCheckerPtr cc(new CollisionChecker());
+ RobotPtr clonedRobot =
+ this->robot->clone("clonedRobot_" + std::to_string(i), cc);
+ // clonedRobot->setUpdateVisualization(false);
- SceneObjectSetPtr dynamic_collision_model = this->dynamicCollisionModel;
- if (dynamicCollisionModel && clonedRobot->hasRobotNodeSet(dynamicCollisionModel->getName()))
- {
- dynamicCollisionModel = clonedRobot->getRobotNodeSet(dynamicCollisionModel->getName());
- }
+ // hack: make sure, the destructor of the clonedRobot is not called.
+ {
+ std::lock_guard g{robotsMtx};
+ robots.push_back(clonedRobot);
+ }
- // now sample some configs and add them to the workspace data
- for (unsigned int j = 0; j < numPosesPerThread; j++)
- {
- float rndValue;
- float minJ, maxJ;
- Eigen::VectorXf v(clonedNodeSet->getSize());
- float maxLoops = 1000;
+ RobotNodeSetPtr clonedNodeSet =
+ clonedRobot->getRobotNodeSet(this->nodeSet->getName());
+ RobotNodePtr clonedTcpNode =
+ clonedRobot->getRobotNode(this->tcpNode->getName());
- bool successfullyRandomized = false;
+ SceneObjectSetPtr static_collision_model = this->staticCollisionModel;
+ if (static_collision_model &&
+ clonedRobot->hasRobotNodeSet(static_collision_model->getName()))
+ {
+ static_collision_model =
+ clonedRobot->getRobotNodeSet(static_collision_model->getName());
+ }
+
+ SceneObjectSetPtr dynamic_collision_model = this->dynamicCollisionModel;
+ if (dynamicCollisionModel &&
+ clonedRobot->hasRobotNodeSet(dynamicCollisionModel->getName()))
+ {
+ dynamicCollisionModel =
+ clonedRobot->getRobotNodeSet(dynamicCollisionModel->getName());
+ }
- for (int k = 0; k < maxLoops; k++)
+ std::cout << "Samples this thread: " << numPosesPerThread;
+
+ // now sample some configs and add them to the workspace data
+ for (unsigned int nValidSamples = 0; nValidSamples < numPosesPerThread;)
{
+ Eigen::VectorXf v(clonedNodeSet->getSize());
+
for (unsigned int l = 0; l < clonedNodeSet->getSize(); l++)
{
- rndValue = RandomFloat(); // value from 0 to 1
- minJ = (*nodeSet)[l]->getJointLimitLo();
- maxJ = (*nodeSet)[l]->getJointLimitHi();
- v[l] = minJ + ((maxJ - minJ) * rndValue);
+ float rndValue = RandomFloat(); // value from 0 to 1
+ float minJ = (*nodeSet)[l]->getJointLimitLo();
+ float maxJ = (*nodeSet)[l]->getJointLimitHi();
+ v[l] = minJ + ((maxJ - minJ) * rndValue);
}
clonedRobot->setJointValues(clonedNodeSet, v);
- // check for collisions
- if (!checkForSelfCollisions || !static_collision_model || !dynamicCollisionModel)
+ const bool successfullyRandomized = [&]() -> bool
{
- successfullyRandomized = true;
- break;
- }
+ // check for collisions
+ if (!checkForSelfCollisions || !static_collision_model ||
+ !dynamicCollisionModel)
+ {
+ return true;
+ }
- if (!clonedRobot->getCollisionChecker()->checkCollision(static_collision_model, dynamicCollisionModel))
+ if (not clonedRobot->getCollisionChecker()->checkCollision(
+ static_collision_model, dynamicCollisionModel))
+ {
+ return true;
+ }
+
+ return false;
+ }();
+
+
+ if (successfullyRandomized)
{
- successfullyRandomized = true;
- break;
- }
+ Eigen::Matrix4f p = clonedTcpNode->getGlobalPose();
+ addPose(p);
- this->collisionConfigs++;
+ nValidSamples++; // increment loop counter
+ }
+ else
+ {
+ VR_WARNING << "Could not find collision-free configuration...";
+ this->collisionConfigs++;
+ }
}
- if (successfullyRandomized)
- {
- Eigen::Matrix4f p = clonedTcpNode->getGlobalPose();
+ VR_WARNING << "Thread " << i << " finished";
- VR_INFO << "Adding pose";
- std::cout << "Adding pose";
- addPose(p);
- }
- else
- {
- VR_WARNING << "Could not find collision-free configuration...";
- }
- }
- });
+
+ });
}
+ std::cout << "waiting for threads to finish";
+
// wait for all threads to finish
for (unsigned int i = 0; i < numThreads; i++)
{
threads[i].join();
}
+
+ std::cout << "finished";
+ robot->setUpdateVisualization(visuSate);
}
} // namespace VirtualRobot
--
GitLab
From 0cd2c2108dffce6cb3f97d3b8c0699717ef86a45 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 8 Jul 2021 15:02:22 +0200
Subject: [PATCH 06/14] soechting nullspace gradient with wrist
---
VirtualRobot/CMakeLists.txt | 2 +
.../SoechtingNullspaceGradient.cpp | 2 +-
.../SoechtingNullspaceGradientWithWrist.cpp | 178 +++++
.../SoechtingNullspaceGradientWithWrist.h | 73 ++
VirtualRobot/Visualization/ColorMap.cpp | 4 +
VirtualRobot/Visualization/ColorMap.h | 2 +-
VirtualRobot/Workspace/NaturalPosture.cpp | 80 +--
.../examples/RobotViewer/DiffIKWidget.cpp | 667 ++++++++++++------
.../reachability/reachabilityWindow.cpp | 7 +-
9 files changed, 746 insertions(+), 269 deletions(-)
create mode 100644 VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
create mode 100644 VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
diff --git a/VirtualRobot/CMakeLists.txt b/VirtualRobot/CMakeLists.txt
index dacef81f8..601d5b22e 100644
--- a/VirtualRobot/CMakeLists.txt
+++ b/VirtualRobot/CMakeLists.txt
@@ -236,6 +236,7 @@ SET(SOURCES
IK/CompositeDiffIK/CompositeDiffIK.cpp
IK/CompositeDiffIK/ManipulabilityNullspaceGradient.cpp
IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
+ IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
Import/RobotImporterFactory.cpp
Import/SimoxXMLFactory.cpp
@@ -451,6 +452,7 @@ SET(INCLUDES
IK/CompositeDiffIK/ManipulabilityNullspaceGradient.h
IK/CompositeDiffIK/Soechting.h
IK/CompositeDiffIK/SoechtingNullspaceGradient.h
+ IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
Import/RobotImporterFactory.h
Import/SimoxXMLFactory.h
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
index 04f7bba22..3d6270cbe 100644
--- a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.cpp
@@ -92,7 +92,7 @@ namespace VirtualRobot
arm.forearmLength = 0.f;
Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
- const auto sa = Soechting::CalculateAngles(targetPosition, arm, 1.0, true);
+ const auto sa = Soechting::CalculateAngles(targetPosition, arm, 1.3, false);
const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
new file mode 100644
index 000000000..9ebfe5e0d
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.cpp
@@ -0,0 +1,178 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Fabian Reister (fabian dot reister at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#include "SoechtingNullspaceGradientWithWrist.h"
+
+#include <cmath>
+
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/Robot.h>
+
+#include "MathTools.h"
+#include "Soechting.h"
+#include "VirtualRobot.h"
+
+namespace VirtualRobot
+{
+ SoechtingNullspaceGradientWithWrist::SoechtingNullspaceGradientWithWrist(
+ const CompositeDiffIK::TargetPtr& target,
+ const std::string& shoulderName,
+ const Soechting::ArmType& arm,
+ const ArmJointsWithWrist& joints) :
+ CompositeDiffIK::NullspaceGradient(joints.getRobotNodeNames()),
+ rns(joints.createRobotNodeSet("SoechtingWithWrist" + std::to_string(arm))),
+ target(target),
+ shoulder(rns->getRobot()->getRobotNode(shoulderName)),
+ arm(arm),
+ joints(joints)
+ {
+ }
+
+ void SoechtingNullspaceGradientWithWrist::init(CompositeDiffIK::Parameters&)
+ {
+ // Do nothing
+ }
+
+ Eigen::VectorXf
+ SoechtingNullspaceGradientWithWrist::getGradient(CompositeDiffIK::Parameters& params,
+ int /*stepNr*/)
+ {
+ const size_t dim = rns->getSize();
+
+ auto sa = calcShoulderAngles(params);
+ Eigen::VectorXf weights = Eigen::VectorXf::Zero(dim);
+ Eigen::VectorXf target = Eigen::VectorXf::Zero(dim);
+
+ const auto set = [&](const auto& joint, const float w, const float t)
+ {
+ if (joint)
+ {
+ auto id = rns->getRobotNodeIndex(joint);
+ VR_ASSERT(id >= 0);
+ weights[id] = w;
+ target[id] = t;
+ }
+ };
+
+ set(joints.clavicula, 0.5f, sa.C);
+ set(joints.shoulder1, 1, sa.SE);
+ set(joints.shoulder2, 2, sa.SR);
+ set(joints.shoulder3, 0.5f, -M_PI / 4);
+ set(joints.elbow, 0.5f, sa.E);
+
+ set(joints.forearm, 0.5f, 0.F);
+
+ set(joints.wristAdduction, 0.5f, MathTools::deg2rad(10));
+
+ switch (arm)
+ {
+ case Soechting::ArmType::Left:
+ set(joints.wristExtension, 0.5F, -MathTools::deg2rad(20));
+ break;
+ case Soechting::ArmType::Right:
+ set(joints.wristExtension, 0.5F, +MathTools::deg2rad(20));
+ break;
+ }
+
+ // std::cout << "target" << target << std::endl;
+ // std::cout << "diff" << (target - rns->getJointValuesEigen()) << std::endl;
+
+ return (target - rns->getJointValuesEigen()).cwiseProduct(weights);
+ }
+
+ SoechtingNullspaceGradientWithWrist::ShoulderAngles
+ SoechtingNullspaceGradientWithWrist::calcShoulderAngles(
+ const CompositeDiffIK::Parameters& /*params*/) const
+ {
+ const Eigen::Matrix4f currentShoulder = shoulder->getPoseInRootFrame();
+
+ Soechting::Arm arm;
+ arm.armType = this->arm;
+ arm.shoulder = currentShoulder.block<3, 1>(0, 3);
+ arm.upperArmLength = 0.F;
+ arm.forearmLength = 0.F;
+
+ Eigen::Vector3f targetPosition = target->target.block<3, 1>(0, 3);
+ const auto sa = Soechting::CalculateAngles(targetPosition, arm, 1.3, false);
+
+ const Eigen::AngleAxisf aaSE(sa.SE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaSY(-sa.SY, Eigen::Vector3f::UnitZ());
+ const Eigen::AngleAxisf aaEE(-sa.EE, Eigen::Vector3f::UnitX());
+ const Eigen::AngleAxisf aaEY(-sa.EY, Eigen::Vector3f::UnitZ());
+
+ const Eigen::Vector3f elb = Eigen::AngleAxisf(-sa.SE, Eigen::Vector3f::UnitX()) * aaSY *
+ aaSE * -Eigen::Vector3f::UnitZ();
+ const float SR = std::atan2(elb(0), -elb(2));
+
+ ShoulderAngles res;
+ res.SR = std::max(-0.1F, SR);
+ res.SE = sa.SE;
+ res.E = sa.EE;
+ res.C = 0;
+ return res;
+ }
+
+ RobotNodeSetPtr SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist::createRobotNodeSet(
+ const std::string& name) const
+ {
+ std::vector<RobotNodePtr> robotNodes;
+ for (const auto& node : {clavicula,
+ shoulder1,
+ shoulder2,
+ shoulder3,
+ elbow,
+ forearm,
+ wristAdduction,
+ wristExtension})
+ {
+ if (node)
+ {
+ robotNodes.push_back(node);
+ }
+ }
+ if (robotNodes.empty())
+ {
+ return nullptr;
+ }
+
+ auto frontNode = robotNodes.front();
+ return RobotNodeSet::createRobotNodeSet(frontNode->getRobot(), name, robotNodes, frontNode);
+ }
+
+ std::vector<std::string>
+ SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist::getRobotNodeNames() const
+ {
+ std::vector<std::string> nodeNames;
+ for (const auto& node : {clavicula,
+ shoulder1,
+ shoulder2,
+ shoulder3,
+ elbow,
+ forearm,
+ wristAdduction,
+ wristExtension})
+ {
+ if (node)
+ {
+ nodeNames.push_back(node->getName());
+ }
+ }
+ return nodeNames;
+ }
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
new file mode 100644
index 000000000..22fc81410
--- /dev/null
+++ b/VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h
@@ -0,0 +1,73 @@
+/**
+* This file is part of Simox.
+*
+* Simox is free software; you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as
+* published by the Free Software Foundation; either version 2 of
+* the License, or (at your option) any later version.
+*
+* Simox is distributed in the hope that it will be useful, but
+* WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU Lesser General Public License for more details.
+*
+* @author Fabian Reister (fabian dot reister at kit dot edu)
+* @copyright http://www.gnu.org/licenses/gpl-2.0.txt
+* GNU General Public License
+*/
+
+#pragma once
+
+#include "CompositeDiffIK.h"
+#include "Soechting.h"
+
+namespace VirtualRobot
+{
+
+ class SoechtingNullspaceGradientWithWrist : public CompositeDiffIK::NullspaceGradient
+ {
+ public:
+ struct ArmJointsWithWrist
+ {
+ RobotNodePtr clavicula = nullptr;
+ RobotNodePtr shoulder1 = nullptr;
+ RobotNodePtr shoulder2 = nullptr;
+ RobotNodePtr shoulder3 = nullptr;
+ RobotNodePtr elbow = nullptr;
+
+ RobotNodePtr forearm = nullptr;
+
+ RobotNodePtr wristAdduction = nullptr;
+ RobotNodePtr wristExtension = nullptr;
+
+ RobotNodeSetPtr createRobotNodeSet(const std::string& name) const;
+ std::vector<std::string> getRobotNodeNames() const;
+ };
+
+ struct ShoulderAngles
+ {
+ float SE, SR, E, C;
+ };
+
+ SoechtingNullspaceGradientWithWrist(const CompositeDiffIK::TargetPtr& target,
+ const std::string& shoulderName,
+ const Soechting::ArmType& arm,
+ const ArmJointsWithWrist& joints);
+ virtual ~SoechtingNullspaceGradientWithWrist() = default;
+
+ void init(CompositeDiffIK::Parameters&) override;
+ Eigen::VectorXf getGradient(CompositeDiffIK::Parameters& params, int stepNr) override;
+
+ RobotNodeSetPtr rns;
+ CompositeDiffIK::TargetPtr target;
+ VirtualRobot::RobotNodePtr shoulder;
+ Soechting::ArmType arm;
+ ArmJointsWithWrist joints;
+
+ ShoulderAngles calcShoulderAngles(const CompositeDiffIK::Parameters& params) const;
+ };
+
+ using SoechtingNullspaceGradientWithWristPtr =
+ std::shared_ptr<SoechtingNullspaceGradientWithWrist>;
+
+} // namespace VirtualRobot
diff --git a/VirtualRobot/Visualization/ColorMap.cpp b/VirtualRobot/Visualization/ColorMap.cpp
index aeda2984e..d6a3591d8 100644
--- a/VirtualRobot/Visualization/ColorMap.cpp
+++ b/VirtualRobot/Visualization/ColorMap.cpp
@@ -130,6 +130,10 @@ namespace VirtualRobot
addColorKey(0, 0, 0, 0, 0.0f);
addColorKey(0, 0, 255, 255, 1.0f);
break;
+ case eGray:
+ addColorKey(255, 255, 255, 255, 0.0f);
+ addColorKey(0, 0, 0, 255, 1.0f);
+ break;
}
sort();
diff --git a/VirtualRobot/Visualization/ColorMap.h b/VirtualRobot/Visualization/ColorMap.h
index e26955722..762ca3746 100644
--- a/VirtualRobot/Visualization/ColorMap.h
+++ b/VirtualRobot/Visualization/ColorMap.h
@@ -36,7 +36,7 @@ namespace VirtualRobot
enum type
{
- eIntensity, eHot, eRed, eGreen, eBlue, eHotAlpha, eRedAlpha, eBlueAlpha, eGreenAlpha
+ eIntensity, eHot, eRed, eGreen, eBlue, eHotAlpha, eRedAlpha, eBlueAlpha, eGreenAlpha, eGray
};
ColorMap(type t);
diff --git a/VirtualRobot/Workspace/NaturalPosture.cpp b/VirtualRobot/Workspace/NaturalPosture.cpp
index 68b96c65e..8de660641 100644
--- a/VirtualRobot/Workspace/NaturalPosture.cpp
+++ b/VirtualRobot/Workspace/NaturalPosture.cpp
@@ -5,6 +5,7 @@
#include <cmath>
#include <fstream>
#include <stdexcept>
+
#include <Eigen/src/Core/Matrix.h>
#include <VirtualRobot/IK/CompositeDiffIK/Soechting.h>
@@ -18,6 +19,7 @@
#include "../VirtualRobotException.h"
#include "IK/CompositeDiffIK/CompositeDiffIK.h"
#include "IK/CompositeDiffIK/SoechtingNullspaceGradient.h"
+#include "IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h"
#include "VirtualRobot.h"
namespace VirtualRobot
@@ -32,11 +34,10 @@ namespace VirtualRobot
{
// auto target1 = ik->addTarget(robot->getRobotNode(tcpNode->getName()), VirtualRobot::IKSolver::All);
}
-
float NaturalPosture::evaluate()
{
- SoechtingNullspaceGradientPtr soechtingNullspaceGradient;
+ SoechtingNullspaceGradientWithWristPtr soechtingNullspaceGradient;
CompositeDiffIK::Parameters params;
@@ -46,14 +47,17 @@ namespace VirtualRobot
if (robot->getName() == "Armar6" && nodeSet->getName() == "RightArm")
{
// std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = robot->getRobotNode("ArmR1_Cla1");
- armjoints.shoulder1 = robot->getRobotNode("ArmR2_Sho1");
- armjoints.shoulder2 = robot->getRobotNode("ArmR3_Sho2");
- armjoints.shoulder3 = robot->getRobotNode("ArmR4_Sho3");
- armjoints.elbow = robot->getRobotNode("ArmR5_Elb1");
-
- soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradient(
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmR5_Elb1");
+ armjoints.forearm = robot->getRobotNode("ArmR6_Elb2");
+ armjoints.wristAdduction = robot->getRobotNode("ArmR7_Wri1");
+ armjoints.wristExtension = robot->getRobotNode("ArmR8_Wri2");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradientWithWrist(
target, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
soechtingNullspaceGradient->kP = 1.0;
// ik->addNullspaceGradient(gradient);
@@ -61,14 +65,17 @@ namespace VirtualRobot
else if (robot->getName() == "Armar6" && nodeSet->getName() == "LeftArm")
{
// std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = robot->getRobotNode("ArmL1_Cla1");
- armjoints.shoulder1 = robot->getRobotNode("ArmL2_Sho1");
- armjoints.shoulder2 = robot->getRobotNode("ArmL3_Sho2");
- armjoints.shoulder3 = robot->getRobotNode("ArmL4_Sho3");
- armjoints.elbow = robot->getRobotNode("ArmL5_Elb1");
-
- soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradient(
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = robot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = robot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = robot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = robot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = robot->getRobotNode("ArmL5_Elb1");
+ armjoints.forearm = robot->getRobotNode("ArmL6_Elb2");
+ armjoints.wristAdduction = robot->getRobotNode("ArmL7_Wri1");
+ armjoints.wristExtension = robot->getRobotNode("ArmL8_Wri2");
+
+ soechtingNullspaceGradient.reset(new VirtualRobot::SoechtingNullspaceGradientWithWrist(
target, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
soechtingNullspaceGradient->kP = 1.0;
@@ -80,24 +87,22 @@ namespace VirtualRobot
}
const auto weightedDiff = soechtingNullspaceGradient->getGradient(params, -1);
- const float e1 = weightedDiff.squaredNorm();
-
+ const float e1 = weightedDiff.squaredNorm();
+ // auto wristAdduction = nodeSet->getNode("ArmR7_Wri1");
+ // auto wristExtension = nodeSet->getNode("ArmR8_Wri2");
- auto wristAdduction = nodeSet->getNode("ArmR7_Wri1");
- auto wristExtension = nodeSet->getNode("ArmR8_Wri2");
-
- const Eigen::Vector2f wristTarget{ M_PI_2f32 + MathTools::deg2rad(10), M_PI_2f32 - MathTools::deg2rad(20)};
- const Eigen::Vector2f wristJointValues{wristAdduction->getJointValue(), wristExtension->getJointValue() };
+ // const Eigen::Vector2f wristTarget{M_PI_2f32 + MathTools::deg2rad(10),
+ // M_PI_2f32 - MathTools::deg2rad(20)};
+ // const Eigen::Vector2f wristJointValues{wristAdduction->getJointValue(),
+ // wristExtension->getJointValue()};
// set(wristAdduction, 1.F, M_PI_2f32 + MathTools::deg2rad(10));
// set(wristExtension, 1.F, M_PI_2f32 - MathTools::deg2rad(20));
- const float e2 = (wristTarget - wristJointValues).squaredNorm();
-
- return std::sqrt(e1 + e2);
-
+ // const float e2 = (wristTarget - wristJointValues).squaredNorm();
+ return std::sqrt(e1);
}
void NaturalPosture::addPose(const Eigen::Matrix4f& pose)
@@ -152,8 +157,6 @@ namespace VirtualRobot
// unsigned char e = (unsigned char)(mSc * (float)UCHAR_MAX + 0.5f);
-
-
//cout<<"m = "<<m<<endl;
//cout<<"mSc = "<<mSc<<endl;
//cout<<"e = "<<int(e)<<endl;
@@ -162,11 +165,9 @@ namespace VirtualRobot
// VR_INFO << "evaluate: " << ee;
-
-
const float mSc = std::min(ee / 10, 1.0F);
- if(mSc > 1.0)
+ if (mSc > 1.0)
{
VR_WARNING << "mSc too large " << mSc;
}
@@ -180,21 +181,20 @@ namespace VirtualRobot
const auto e = static_cast<unsigned char>(mSc * static_cast<float>(UCHAR_MAX) + 0.5F);
const auto oldVal = data->get(v);
- if (oldVal == 0) // unset
+ if (oldVal == 0) // if unset
{
data->setDatum(v, e);
}
else
{
- if(e < oldVal)
+ if (e < oldVal)
{
data->setDatum(v, e);
}
}
-
-
-
- }else {
+ }
+ else
+ {
VR_WARNING << "Could not get voxel from pose!";
}
diff --git a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
index 8822dc756..a5fe3160f 100644
--- a/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
+++ b/VirtualRobot/examples/RobotViewer/DiffIKWidget.cpp
@@ -1,27 +1,30 @@
#include "DiffIKWidget.h"
-#include "ui_DiffIKWidget.h"
#include <QDialog>
-#include <QVBoxLayout>
#include <QRegExp>
-#include <Inventor/nodes/SoMatrixTransform.h>
-#include <Inventor/nodes/SoUnits.h>
+#include <QThread>
+#include <QVBoxLayout>
+
#include <SimoxUtility/algorithm/string/string_conversion.h>
#include <SimoxUtility/math/convert.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
-#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
+#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
+#include <VirtualRobot/IK/DifferentialIK.h>
#include <VirtualRobot/Manipulability/BimanualManipulability.h>
#include <VirtualRobot/Manipulability/BimanualManipulabilityTracking.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
-#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulability.h>
+#include <VirtualRobot/Manipulability/SingleChainManipulabilityTracking.h>
#include <VirtualRobot/Robot.h>
-#include <VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradient.h>
-#include <VirtualRobot/IK/DifferentialIK.h>
-#include <QThread>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationNode.h>
+
+#include "VirtualRobot/IK/CompositeDiffIK/SoechtingNullspaceGradientWithWrist.h"
+#include "ui_DiffIKWidget.h"
+#include <Inventor/nodes/SoMatrixTransform.h>
+#include <Inventor/nodes/SoUnits.h>
DiffIKWidget* DiffIKWidget::diffIKWidget = nullptr;
-DiffIKWidget::DiffIKWidget(SoSeparator *sceneSep, QDialog *parent) :
+DiffIKWidget::DiffIKWidget(SoSeparator* sceneSep, QDialog* parent) :
QWidget(parent),
dialog(parent),
ui(new Ui::DiffIKWidget),
@@ -38,26 +41,66 @@ DiffIKWidget::DiffIKWidget(SoSeparator *sceneSep, QDialog *parent) :
sceneSep->addChild(followManipSep);
sceneSep->addChild(endeffectorSep);
- connect(ui->comboBoxRNS, SIGNAL(currentTextChanged(QString)), this, SLOT(setRobotNodeSet(QString)));
- connect(ui->comboBoxRNS2, SIGNAL(currentTextChanged(QString)), this, SLOT(setRobotNodeSet2(QString)));
+ connect(
+ ui->comboBoxRNS, SIGNAL(currentTextChanged(QString)), this, SLOT(setRobotNodeSet(QString)));
+ connect(ui->comboBoxRNS2,
+ SIGNAL(currentTextChanged(QString)),
+ this,
+ SLOT(setRobotNodeSet2(QString)));
connect(ui->printJacobianButton, SIGNAL(clicked()), this, SLOT(printJacobian()));
connect(ui->printJacobian2Button, SIGNAL(clicked()), this, SLOT(printJacobian2()));
- connect(ui->checkBoxVisManip, SIGNAL(toggled(bool)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(ui->checkBoxBimanual, SIGNAL(toggled(bool)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(ui->comboBoxManip, SIGNAL(currentTextChanged(QString)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(ui->comboBoxManipType, SIGNAL(currentTextChanged(QString)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(ui->elliosoidScaling, SIGNAL(valueChanged(int)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(ui->ellipsoidTransparency, SIGNAL(valueChanged(double)), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
- connect(this, SIGNAL(currentManipUpdated()), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
-
- connect(ui->followBox, SIGNAL(toggled(bool)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(ui->comboBoxManip, SIGNAL(currentTextChanged(QString)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(ui->elliosoidScaling, SIGNAL(valueChanged(int)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(ui->ellipsoidTransparency, SIGNAL(valueChanged(double)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(this, SIGNAL(currentManipUpdated()), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(ui->followManipulability, SIGNAL(textChanged()), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
- connect(ui->checkBoxBimanual, SIGNAL(toggled(bool)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->checkBoxVisManip,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->checkBoxBimanual,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManip,
+ SIGNAL(currentTextChanged(QString)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManipType,
+ SIGNAL(currentTextChanged(QString)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->elliosoidScaling,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(ui->ellipsoidTransparency,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(updateCurrentManipulabilityEllipsoidVis()));
+ connect(
+ this, SIGNAL(currentManipUpdated()), this, SLOT(updateCurrentManipulabilityEllipsoidVis()));
+
+ connect(
+ ui->followBox, SIGNAL(toggled(bool)), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->comboBoxManip,
+ SIGNAL(currentTextChanged(QString)),
+ this,
+ SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->elliosoidScaling,
+ SIGNAL(valueChanged(int)),
+ this,
+ SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->ellipsoidTransparency,
+ SIGNAL(valueChanged(double)),
+ this,
+ SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(
+ this, SIGNAL(currentManipUpdated()), this, SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->followManipulability,
+ SIGNAL(textChanged()),
+ this,
+ SLOT(updateFollowManipulabilityEllipsoidVis()));
+ connect(ui->checkBoxBimanual,
+ SIGNAL(toggled(bool)),
+ this,
+ SLOT(updateFollowManipulabilityEllipsoidVis()));
connect(ui->stepButton, SIGNAL(clicked()), this, SLOT(stepFollowManip()));
connect(ui->solveButton, SIGNAL(clicked()), this, SLOT(followManip()));
@@ -78,21 +121,26 @@ DiffIKWidget::DiffIKWidget(SoSeparator *sceneSep, QDialog *parent) :
connect(ui->setAverageJointValues, SIGNAL(clicked()), this, SLOT(setAverageJointValues()));
connect(ui->setRandomJointValues, SIGNAL(clicked()), this, SLOT(setRandomJointValues()));
- qRegisterMetaType<VirtualRobot::AbstractManipulabilityTrackingPtr>("VirtualRobot::AbstractManipulabilityTrackingPtr");
+ qRegisterMetaType<VirtualRobot::AbstractManipulabilityTrackingPtr>(
+ "VirtualRobot::AbstractManipulabilityTrackingPtr");
qRegisterMetaType<Eigen::MatrixXd>("Eigen::MatrixXd");
qRegisterMetaType<Eigen::VectorXf>("Eigen::VectorXf");
qRegisterMetaType<std::map<std::string, float>>("std::map<std::string, float>");
qRegisterMetaType<VirtualRobot::CompositeDiffIKPtr>("VirtualRobot::CompositeDiffIKPtr");
- qRegisterMetaType<VirtualRobot::NullspaceManipulabilityPtr>("VirtualRobot::NullspaceManipulabilityPtr");
+ qRegisterMetaType<VirtualRobot::NullspaceManipulabilityPtr>(
+ "VirtualRobot::NullspaceManipulabilityPtr");
qRegisterMetaType<VirtualRobot::RobotNodeSetPtr>("VirtualRobot::RobotNodeSetPtr");
- Worker *worker = new Worker;
+ Worker* worker = new Worker;
worker->moveToThread(&workerThread);
connect(&workerThread, &QThread::finished, worker, &QObject::deleteLater);
connect(this, &DiffIKWidget::followManipAsync, worker, &Worker::followManip);
connect(this, &DiffIKWidget::solveIKAsync, worker, &Worker::solveIK);
connect(worker, &Worker::distanceUpdated, this, &DiffIKWidget::updateDistance);
- connect(worker, &Worker::currentManipUpdated, this, &DiffIKWidget::updateCurrentManipulabilityEllipsoidVis);
+ connect(worker,
+ &Worker::currentManipUpdated,
+ this,
+ &DiffIKWidget::updateCurrentManipulabilityEllipsoidVis);
connect(worker, &Worker::jointValuesUpdated, this, &DiffIKWidget::updateJointValues);
connect(worker, &Worker::finished, this, &DiffIKWidget::workerFinished);
workerThread.start();
@@ -108,24 +156,30 @@ DiffIKWidget::~DiffIKWidget()
delete ui;
}
-QDialog *DiffIKWidget::getDialog() {
+QDialog* DiffIKWidget::getDialog()
+{
return dialog;
}
-void DiffIKWidget::open(QWidget *parent, SoSeparator *sceneSep) {
- if (!diffIKWidget) {
- auto diffIKDialog = new QDialog(parent);
+void DiffIKWidget::open(QWidget* parent, SoSeparator* sceneSep)
+{
+ if (!diffIKWidget)
+ {
+ auto diffIKDialog = new QDialog(parent);
QVBoxLayout* layout = new QVBoxLayout();
- diffIKWidget = new DiffIKWidget(sceneSep, diffIKDialog);
+ diffIKWidget = new DiffIKWidget(sceneSep, diffIKDialog);
layout->addWidget(diffIKWidget);
diffIKDialog->setLayout(layout);
}
diffIKWidget->getDialog()->show();
}
-void DiffIKWidget::update(VirtualRobot::RobotPtr robot) {
- if (diffIKWidget && diffIKWidget->getDialog()->isVisible()) {
- if (diffIKWidget->robot != robot) {
+void DiffIKWidget::update(VirtualRobot::RobotPtr robot)
+{
+ if (diffIKWidget && diffIKWidget->getDialog()->isVisible())
+ {
+ if (diffIKWidget->robot != robot)
+ {
diffIKWidget->robot = robot;
diffIKWidget->addRobotNodeSets();
}
@@ -135,84 +189,114 @@ void DiffIKWidget::update(VirtualRobot::RobotPtr robot) {
}
}
-void DiffIKWidget::close() {
+void DiffIKWidget::close()
+{
delete diffIKWidget;
diffIKWidget = nullptr;
}
-
-Eigen::MatrixXd DiffIKWidget::readFollowManipulability() {
- try {
+Eigen::MatrixXd DiffIKWidget::readFollowManipulability()
+{
+ try
+ {
Eigen::MatrixXd matrix;
QString data = ui->followManipulability->toPlainText();
// QTextEdit content
QStringList strList = data.split(QRegExp("[\n]"), QString::SkipEmptyParts);
- if (strList.size() == 6) {
+ if (strList.size() == 6)
+ {
matrix = Eigen::Matrix<double, 6, 6>();
}
- else if (strList.size() == 3) {
+ else if (strList.size() == 3)
+ {
matrix = Eigen::Matrix3d();
}
- else {
+ else
+ {
return Eigen::Matrix<double, 0, 0>();
}
matrix.setZero();
- for (int i = 0; i < strList.size(); i++) {
+ for (int i = 0; i < strList.size(); i++)
+ {
QStringList s = strList[i].split(QRegExp(" "), QString::SkipEmptyParts);
- for (int j = 0; j < s.size(); j++) {
- float value = simox::alg::to_<double>(s[j].toStdString());
- matrix(i,j) = value;
- if (j > matrix.cols()) break;
+ for (int j = 0; j < s.size(); j++)
+ {
+ float value = simox::alg::to_<double>(s[j].toStdString());
+ matrix(i, j) = value;
+ if (j > matrix.cols())
+ break;
}
- if (i > matrix.rows()) break;
+ if (i > matrix.rows())
+ break;
}
return matrix;
}
- catch (...) {
+ catch (...)
+ {
return Eigen::Matrix<double, 0, 0>();
}
}
-VirtualRobot::AbstractManipulability::Mode DiffIKWidget::getMode(QComboBox *comboBox) {
- if (comboBox->currentText() == "Whole") {
+VirtualRobot::AbstractManipulability::Mode DiffIKWidget::getMode(QComboBox* comboBox)
+{
+ if (comboBox->currentText() == "Whole")
+ {
return VirtualRobot::AbstractManipulability::Whole;
}
- else if (comboBox->currentText() == "Position") {
+ else if (comboBox->currentText() == "Position")
+ {
return VirtualRobot::AbstractManipulability::Position;
}
- else if (comboBox->currentText() == "Rotation") {
+ else if (comboBox->currentText() == "Rotation")
+ {
return VirtualRobot::AbstractManipulability::Orientation;
}
- else {
+ else
+ {
throw std::runtime_error("Wrong!");
}
}
-VirtualRobot::AbstractManipulability::Type DiffIKWidget::getManipulabilityType(QComboBox *comboBox) {
- if (comboBox->currentText() == "Velocity") {
+VirtualRobot::AbstractManipulability::Type DiffIKWidget::getManipulabilityType(QComboBox* comboBox)
+{
+ if (comboBox->currentText() == "Velocity")
+ {
return VirtualRobot::AbstractManipulability::Velocity;
}
- else if (comboBox->currentText() == "Force") {
+ else if (comboBox->currentText() == "Force")
+ {
return VirtualRobot::AbstractManipulability::Force;
}
- else {
+ else
+ {
throw std::runtime_error("Wrong!");
}
}
-Eigen::Matrix4f DiffIKWidget::getEndEffectorPos() {
- return simox::math::pos_rpy_to_mat4f(ui->xTarget->value(), ui->yTarget->value(), ui->zTarget->value(),
- ui->rollTarget->value(), ui->pitchTarget->value(), ui->yawTarget->value());
+Eigen::Matrix4f DiffIKWidget::getEndEffectorPos()
+{
+ return simox::math::pos_rpy_to_mat4f(ui->xTarget->value(),
+ ui->yTarget->value(),
+ ui->zTarget->value(),
+ ui->rollTarget->value(),
+ ui->pitchTarget->value(),
+ ui->yawTarget->value());
}
-Eigen::Matrix4f DiffIKWidget::getEndEffectorPos2() {
- return simox::math::pos_rpy_to_mat4f(ui->xTarget2->value(), ui->yTarget2->value(), ui->zTarget2->value(),
- ui->rollTarget2->value(), ui->pitchTarget2->value(), ui->yawTarget2->value());
+Eigen::Matrix4f DiffIKWidget::getEndEffectorPos2()
+{
+ return simox::math::pos_rpy_to_mat4f(ui->xTarget2->value(),
+ ui->yTarget2->value(),
+ ui->zTarget2->value(),
+ ui->rollTarget2->value(),
+ ui->pitchTarget2->value(),
+ ui->yawTarget2->value());
}
-void DiffIKWidget::addRobotNodeSets() {
+void DiffIKWidget::addRobotNodeSets()
+{
ui->comboBoxRNS->clear();
for (auto& robotNodeSet : robot->getRobotNodeSets())
{
@@ -229,18 +313,23 @@ void DiffIKWidget::addRobotNodeSets() {
ui->comboBoxRNS2->setCurrentIndex(0);
}
-void DiffIKWidget::updateCurrentManipulabilityEllipsoidVis() {
+void DiffIKWidget::updateCurrentManipulabilityEllipsoidVis()
+{
manipSep->removeAllChildren();
ui->currentManipulability->clear();
- if (ui->checkBoxVisManip->isChecked()) {
+ if (ui->checkBoxVisManip->isChecked())
+ {
auto manipTracking = getManipulabilityTracking();
- if (!manipTracking) return;
+ if (!manipTracking)
+ return;
auto manipulability = manipTracking->computeCurrentManipulability();
- VirtualRobot::VisualizationNodePtr visNode = manipTracking->getManipulabilityVis(manipulability, "", ui->elliosoidScaling->value());
+ VirtualRobot::VisualizationNodePtr visNode =
+ manipTracking->getManipulabilityVis(manipulability, "", ui->elliosoidScaling->value());
auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
- if (coinVisNode) {
+ if (coinVisNode)
+ {
auto coinVis = coinVisNode->getCoinVisualization();
- auto mat = new SoMaterial();
+ auto mat = new SoMaterial();
mat->transparency.setValue(ui->ellipsoidTransparency->value());
manipSep->addChild(mat);
manipSep->addChild(coinVis);
@@ -251,23 +340,30 @@ void DiffIKWidget::updateCurrentManipulabilityEllipsoidVis() {
}
}
-void DiffIKWidget::updateFollowManipulabilityEllipsoidVis() {
+void DiffIKWidget::updateFollowManipulabilityEllipsoidVis()
+{
followManipSep->removeAllChildren();
- if (ui->followBox->isChecked() && currentRobotNodeSet) {
- Eigen::MatrixXd followManip = readFollowManipulability();
- VirtualRobot::RobotNodeSetPtr rns = nullptr;
+ if (ui->followBox->isChecked() && currentRobotNodeSet)
+ {
+ Eigen::MatrixXd followManip = readFollowManipulability();
+ VirtualRobot::RobotNodeSetPtr rns = nullptr;
VirtualRobot::AbstractManipulabilityTrackingPtr tracking = getManipulabilityTracking();
- if (tracking) {
- if (followManip.rows() != tracking->getTaskVars()) return;
+ if (tracking)
+ {
+ if (followManip.rows() != tracking->getTaskVars())
+ return;
double distance = tracking->computeDistance(followManip);
emit distanceUpdated(distance);
- VirtualRobot::VisualizationNodePtr visNode = tracking->getManipulabilityVis(followManip, "", ui->elliosoidScaling->value());
- auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
- if (coinVisNode) {
+ VirtualRobot::VisualizationNodePtr visNode =
+ tracking->getManipulabilityVis(followManip, "", ui->elliosoidScaling->value());
+ auto coinVisNode =
+ std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
+ if (coinVisNode)
+ {
auto coinVis = coinVisNode->getCoinVisualization();
- auto mat = new SoMaterial();
+ auto mat = new SoMaterial();
mat->transparency.setValue(ui->ellipsoidTransparency->value());
followManipSep->addChild(mat);
followManipSep->addChild(coinVis);
@@ -276,19 +372,23 @@ void DiffIKWidget::updateFollowManipulabilityEllipsoidVis() {
}
}
-void DiffIKWidget::updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotNodeSet, const Eigen::Matrix4f &pose) {
- if (robotNodeSet) {
+void DiffIKWidget::updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotNodeSet,
+ const Eigen::Matrix4f& pose)
+{
+ if (robotNodeSet)
+ {
auto tcp = robotNodeSet->getTCP();
- if (tcp) {
+ if (tcp)
+ {
SoSeparator* sep = new SoSeparator();
// Set the visualization stuff to milimeters
- SoUnits *u = new SoUnits();
- u->units = SoUnits::MILLIMETERS;
+ SoUnits* u = new SoUnits();
+ u->units = SoUnits::MILLIMETERS;
sep->addChild(u);
// set a transformation matrix for the visualization
Eigen::Matrix4f transformation = robotNodeSet->getRobot()->getGlobalPose(pose);
- SoMatrixTransform* mt = new SoMatrixTransform();
+ SoMatrixTransform* mt = new SoMatrixTransform();
SbMatrix m_(reinterpret_cast<SbMat*>(transformation.data()));
mt->matrix.setValue(m_);
sep->addChild(mt);
@@ -297,18 +397,24 @@ void DiffIKWidget::updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotN
mat->transparency.setValue(0.5);
sep->addChild(mat);
- if (ui->checkBoxOriIK->isChecked()) {
+ if (ui->checkBoxOriIK->isChecked())
+ {
auto visNode = tcp->getVisualization(VirtualRobot::SceneObject::Full);
- auto coinVisNode = std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
- if (coinVisNode) {
+ auto coinVisNode =
+ std::dynamic_pointer_cast<VirtualRobot::CoinVisualizationNode>(visNode);
+ if (coinVisNode)
+ {
auto coinVis = coinVisNode->getCoinVisualization();
sep->addChild(coinVis);
}
- else {
- sep->addChild(VirtualRobot::CoinVisualizationFactory::CreateSphere(50, 1, 0, 0));
+ else
+ {
+ sep->addChild(
+ VirtualRobot::CoinVisualizationFactory::CreateSphere(50, 1, 0, 0));
}
}
- else {
+ else
+ {
sep->addChild(VirtualRobot::CoinVisualizationFactory::CreateSphere(50, 1, 0, 0));
}
endeffectorSep->addChild(sep);
@@ -316,132 +422,183 @@ void DiffIKWidget::updateEndeffectorPoseVis(VirtualRobot::RobotNodeSetPtr robotN
}
}
-VirtualRobot::AbstractManipulabilityTrackingPtr DiffIKWidget::getManipulabilityTracking() {
+VirtualRobot::AbstractManipulabilityTrackingPtr DiffIKWidget::getManipulabilityTracking()
+{
return getManipulabilityTracking(currentRobotNodeSet, currentRobotNodeSet2);
}
-VirtualRobot::AbstractManipulabilityTrackingPtr DiffIKWidget::getManipulabilityTracking(VirtualRobot::RobotNodeSetPtr r1, VirtualRobot::RobotNodeSetPtr r2, bool setNewRNS) {
+VirtualRobot::AbstractManipulabilityTrackingPtr
+DiffIKWidget::getManipulabilityTracking(VirtualRobot::RobotNodeSetPtr r1,
+ VirtualRobot::RobotNodeSetPtr r2,
+ bool setNewRNS)
+{
VirtualRobot::AbstractManipulabilityTrackingPtr tracking = nullptr;
- if (r1 && r1->getTCP()) {
+ if (r1 && r1->getTCP())
+ {
auto mode = getMode(ui->comboBoxManip);
- if (!ui->checkBoxBimanual->isChecked()) {
- VirtualRobot::SingleRobotNodeSetManipulabilityPtr manip(new VirtualRobot::SingleRobotNodeSetManipulability(r1, mode, getManipulabilityType(ui->comboBoxManipType)));
- if (setNewRNS) newRNS = manip->getRobotNodeSet();
- tracking = VirtualRobot::SingleChainManipulabilityTrackingPtr(new VirtualRobot::SingleChainManipulabilityTracking(manip));
+ if (!ui->checkBoxBimanual->isChecked())
+ {
+ VirtualRobot::SingleRobotNodeSetManipulabilityPtr manip(
+ new VirtualRobot::SingleRobotNodeSetManipulability(
+ r1, mode, getManipulabilityType(ui->comboBoxManipType)));
+ if (setNewRNS)
+ newRNS = manip->getRobotNodeSet();
+ tracking = VirtualRobot::SingleChainManipulabilityTrackingPtr(
+ new VirtualRobot::SingleChainManipulabilityTracking(manip));
}
- else if (r2 && r2->getTCP() && r1 != r2) {
- VirtualRobot::BimanualManipulabilityPtr manip(new VirtualRobot::BimanualManipulability(r1, r2, mode, getManipulabilityType(ui->comboBoxManipType)));
- if (setNewRNS) newRNS = manip->createRobotNodeSet();
- tracking = VirtualRobot::BimanualManipulabilityTrackingPtr(new VirtualRobot::BimanualManipulabilityTracking(manip));
+ else if (r2 && r2->getTCP() && r1 != r2)
+ {
+ VirtualRobot::BimanualManipulabilityPtr manip(new VirtualRobot::BimanualManipulability(
+ r1, r2, mode, getManipulabilityType(ui->comboBoxManipType)));
+ if (setNewRNS)
+ newRNS = manip->createRobotNodeSet();
+ tracking = VirtualRobot::BimanualManipulabilityTrackingPtr(
+ new VirtualRobot::BimanualManipulabilityTracking(manip));
}
}
return tracking;
}
-void DiffIKWidget::updateEndeffectorPoseVis() {
+void DiffIKWidget::updateEndeffectorPoseVis()
+{
endeffectorSep->removeAllChildren();
- if (ui->checkBoxVisTarget->isChecked()) {
+ if (ui->checkBoxVisTarget->isChecked())
+ {
updateEndeffectorPoseVis(currentRobotNodeSet, getEndEffectorPos());
updateEndeffectorPoseVis(currentRobotNodeSet2, getEndEffectorPos2());
}
- if (ui->checkBoxSolveContinuous->isChecked()) {
+ if (ui->checkBoxSolveContinuous->isChecked())
+ {
solveIK(true);
}
}
-void DiffIKWidget::stepFollowManip() {
+void DiffIKWidget::stepFollowManip()
+{
auto manipTracking = getManipulabilityTracking(currentRobotNodeSet, currentRobotNodeSet2, true);
- if (!manipTracking) return;
+ if (!manipTracking)
+ return;
Eigen::MatrixXd followManip = readFollowManipulability();
- if (followManip.rows() != manipTracking->getTaskVars()) {
+ if (followManip.rows() != manipTracking->getTaskVars())
+ {
std::cout << "Wrong manipulability matrix!" << std::endl;
return;
}
- Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
+ Eigen::VectorXf velocity =
+ manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
std::cout << "Nullspace velocities without gain:\n" << velocity << "\n" << std::endl;
- Eigen::VectorXf jointValues = newRNS->getJointValuesEigen() + velocity * ui->kGain->value();;
+ Eigen::VectorXf jointValues = newRNS->getJointValuesEigen() + velocity * ui->kGain->value();
+ ;
newRNS->setJointValues(jointValues);
double distance = manipTracking->computeDistance(followManip);
emit distanceUpdated(distance);
emit currentManipUpdated();
}
-void DiffIKWidget::followManip() {
- if (!currentRobotNodeSet) {
+void DiffIKWidget::followManip()
+{
+ if (!currentRobotNodeSet)
+ {
std::cout << "RobotNodeSet is null" << std::endl;
return;
}
- clonedRobot = currentRobotNodeSet->getRobot()->clone();
+ clonedRobot = currentRobotNodeSet->getRobot()->clone();
auto clonedRobotNodeSet = clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName());
- auto manipTracking = getManipulabilityTracking(clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
- currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName()) : nullptr,
- true);
- if (!manipTracking) return;
+ auto manipTracking = getManipulabilityTracking(
+ clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
+ currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName())
+ : nullptr,
+ true);
+ if (!manipTracking)
+ return;
Eigen::MatrixXd followManip = readFollowManipulability();
- if (followManip.rows() != manipTracking->getTaskVars()) {
+ if (followManip.rows() != manipTracking->getTaskVars())
+ {
std::cout << "Wrong manipulability matrix!" << std::endl;
return;
}
float maxDistance = ui->maxDistance->value();
- float kGain = ui->kGain->value();
+ float kGain = ui->kGain->value();
ui->solveButton->setEnabled(false);
ui->solveIKButton->setEnabled(false);
emit followManipAsync(manipTracking, newRNS, followManip, kGain, maxDistance);
}
-void DiffIKWidget::updateDistance(double distance) {
+void DiffIKWidget::updateDistance(double distance)
+{
ui->currentDistance->setText(QString::fromStdString(std::to_string(distance)));
}
-void DiffIKWidget::updateJointValues(const std::map<std::string, float> &jointValues) {
+void DiffIKWidget::updateJointValues(const std::map<std::string, float>& jointValues)
+{
currentRobotNodeSet->getRobot()->setJointValues(jointValues);
updateFollowManipulabilityEllipsoidVis();
}
-void DiffIKWidget::solveIK(bool untilReached) {
- if (!currentRobotNodeSet || !currentRobotNodeSet->getTCP()) {
+void DiffIKWidget::solveIK(bool untilReached)
+{
+ if (!currentRobotNodeSet || !currentRobotNodeSet->getTCP())
+ {
std::cout << "RobotNodeSet or TCP is null" << std::endl;
return;
}
std::cout << "Solving IK ..." << std::endl;
- clonedRobot = currentRobotNodeSet->getRobot()->clone();
+ clonedRobot = currentRobotNodeSet->getRobot()->clone();
VirtualRobot::RobotNodeSetPtr clonedRobotNodeSet = nullptr;
- if (ui->checkBoxBimanual->isChecked()) {
- if (currentRobotNodeSet == currentRobotNodeSet2) return;
- std::vector<std::string> robotNodeNames = currentRobotNodeSet->getNodeNames();
+ if (ui->checkBoxBimanual->isChecked())
+ {
+ if (currentRobotNodeSet == currentRobotNodeSet2)
+ return;
+ std::vector<std::string> robotNodeNames = currentRobotNodeSet->getNodeNames();
std::vector<std::string> robotNodeNames2 = currentRobotNodeSet2->getNodeNames();
robotNodeNames.insert(robotNodeNames.end(), robotNodeNames2.begin(), robotNodeNames2.end());
- clonedRobotNodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(clonedRobot, "New", robotNodeNames, clonedRobot->getRootNode()->getName());
+ clonedRobotNodeSet = VirtualRobot::RobotNodeSet::createRobotNodeSet(
+ clonedRobot, "New", robotNodeNames, clonedRobot->getRootNode()->getName());
}
- else clonedRobotNodeSet = clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName());
+ else
+ clonedRobotNodeSet = clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName());
auto tcp = currentRobotNodeSet->getTCP();
- if (!tcp) return;
+ if (!tcp)
+ return;
VirtualRobot::CompositeDiffIKPtr ik(new VirtualRobot::CompositeDiffIK(clonedRobotNodeSet));
Eigen::Matrix4f pose = getEndEffectorPos();
- auto target1 = ik->addTarget(clonedRobot->getRobotNode(tcp->getName()), pose, ui->checkBoxOriIK->isChecked() ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+ auto target1 = ik->addTarget(clonedRobot->getRobotNode(tcp->getName()),
+ pose,
+ ui->checkBoxOriIK->isChecked() ? VirtualRobot::IKSolver::All
+ : VirtualRobot::IKSolver::Position);
- if (ui->checkBoxBimanual->isChecked()) {
+ if (ui->checkBoxBimanual->isChecked())
+ {
auto tcp = currentRobotNodeSet2->getTCP();
- if (tcp) {
- auto node = clonedRobot->getRobotNode(tcp->getName());
+ if (tcp)
+ {
+ auto node = clonedRobot->getRobotNode(tcp->getName());
Eigen::Matrix4f pose = getEndEffectorPos2();
- ik->addTarget(node, pose, ui->checkBoxOriIK2->isChecked() ? VirtualRobot::IKSolver::All : VirtualRobot::IKSolver::Position);
+ ik->addTarget(node,
+ pose,
+ ui->checkBoxOriIK2->isChecked() ? VirtualRobot::IKSolver::All
+ : VirtualRobot::IKSolver::Position);
}
- else return;
+ else
+ return;
}
float jointLimitAvoidance = ui->jointLimitAvoidance->value();
- if (ui->checkBoxJointLimitAvoidance->isChecked() && jointLimitAvoidance > 0) {
- VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(clonedRobotNodeSet));
+ if (ui->checkBoxJointLimitAvoidance->isChecked() && jointLimitAvoidance > 0)
+ {
+ VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidancePtr nsjla(
+ new VirtualRobot::CompositeDiffIK::NullspaceJointLimitAvoidance(clonedRobotNodeSet));
nsjla->kP = jointLimitAvoidance;
- for (auto node : clonedRobotNodeSet->getAllRobotNodes()) {
- if (node->isLimitless()) {
+ for (auto node : clonedRobotNodeSet->getAllRobotNodes())
+ {
+ if (node->isLimitless())
+ {
nsjla->setWeight(node->getName(), 0);
}
}
@@ -449,66 +606,89 @@ void DiffIKWidget::solveIK(bool untilReached) {
}
VirtualRobot::NullspaceManipulabilityPtr nsman = nullptr;
- float kGain = ui->kGainNullspace->value();
- if (ui->checkBoxManipulabilityNullspace->isChecked() && kGain > 0) {
+ float kGain = ui->kGainNullspace->value();
+ if (ui->checkBoxManipulabilityNullspace->isChecked() && kGain > 0)
+ {
std::cout << "Adding manipulability as nullspace target" << std::endl;
- auto manipTracking = getManipulabilityTracking(clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
- currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName()) : nullptr);
- if (!manipTracking) {
+ auto manipTracking = getManipulabilityTracking(
+ clonedRobot->getRobotNodeSet(currentRobotNodeSet->getName()),
+ currentRobotNodeSet2 ? clonedRobot->getRobotNodeSet(currentRobotNodeSet2->getName())
+ : nullptr);
+ if (!manipTracking)
+ {
std::cout << "Manip tracking zero!" << std::endl;
return;
}
Eigen::MatrixXd followManip = readFollowManipulability();
- if (followManip.rows() != manipTracking->getTaskVars()) {
+ if (followManip.rows() != manipTracking->getTaskVars())
+ {
std::cout << "Wrong manipulability matrix!" << std::endl;
return;
}
- nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(manipTracking, followManip, Eigen::MatrixXd(), true));
+ nsman = VirtualRobot::NullspaceManipulabilityPtr(new VirtualRobot::NullspaceManipulability(
+ manipTracking, followManip, Eigen::MatrixXd(), true));
nsman->kP = kGain;
ik->addNullspaceGradient(nsman);
}
float kSoechting = ui->kSoechting->value();
- if (ui->checkBoxSoechtingNullspace->isChecked() && kSoechting > 0) {
- if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "RightArm") {
+ if (ui->checkBoxSoechtingNullspace->isChecked() && kSoechting > 0)
+ {
+ if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "RightArm")
+ {
// std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = clonedRobot->getRobotNode("ArmR1_Cla1");
- armjoints.shoulder1 = clonedRobot->getRobotNode("ArmR2_Sho1");
- armjoints.shoulder2 = clonedRobot->getRobotNode("ArmR3_Sho2");
- armjoints.shoulder3 = clonedRobot->getRobotNode("ArmR4_Sho3");
- armjoints.elbow = clonedRobot->getRobotNode("ArmR5_Elb1");
-
- VirtualRobot::SoechtingNullspaceGradientPtr gradient(new VirtualRobot::SoechtingNullspaceGradient(target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = clonedRobot->getRobotNode("ArmR1_Cla1");
+ armjoints.shoulder1 = clonedRobot->getRobotNode("ArmR2_Sho1");
+ armjoints.shoulder2 = clonedRobot->getRobotNode("ArmR3_Sho2");
+ armjoints.shoulder3 = clonedRobot->getRobotNode("ArmR4_Sho3");
+ armjoints.elbow = clonedRobot->getRobotNode("ArmR5_Elb1");
+ armjoints.forearm = clonedRobot->getRobotNode("ArmR6_Elb2");
+ armjoints.wristAdduction = clonedRobot->getRobotNode("ArmR7_Wri1");
+ armjoints.wristExtension = clonedRobot->getRobotNode("ArmR8_Wri2");
+
+ VirtualRobot::SoechtingNullspaceGradientWithWristPtr gradient(
+ new VirtualRobot::SoechtingNullspaceGradientWithWrist(
+ target1, "ArmR2_Sho1", VirtualRobot::Soechting::ArmType::Right, armjoints));
gradient->kP = kSoechting;
ik->addNullspaceGradient(gradient);
}
- else if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "LeftArm") {
+ else if (robot->getName() == "Armar6" && currentRobotNodeSet->getName() == "LeftArm")
+ {
// std::cout << "Adding soechting nullspace" << std::endl;
- VirtualRobot::SoechtingNullspaceGradient::ArmJoints armjoints;
- armjoints.clavicula = clonedRobot->getRobotNode("ArmL1_Cla1");
- armjoints.shoulder1 = clonedRobot->getRobotNode("ArmL2_Sho1");
- armjoints.shoulder2 = clonedRobot->getRobotNode("ArmL3_Sho2");
- armjoints.shoulder3 = clonedRobot->getRobotNode("ArmL4_Sho3");
- armjoints.elbow = clonedRobot->getRobotNode("ArmL5_Elb1");
-
- VirtualRobot::SoechtingNullspaceGradientPtr gradient(new VirtualRobot::SoechtingNullspaceGradient(target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
+ VirtualRobot::SoechtingNullspaceGradientWithWrist::ArmJointsWithWrist armjoints;
+ armjoints.clavicula = clonedRobot->getRobotNode("ArmL1_Cla1");
+ armjoints.shoulder1 = clonedRobot->getRobotNode("ArmL2_Sho1");
+ armjoints.shoulder2 = clonedRobot->getRobotNode("ArmL3_Sho2");
+ armjoints.shoulder3 = clonedRobot->getRobotNode("ArmL4_Sho3");
+ armjoints.elbow = clonedRobot->getRobotNode("ArmL5_Elb1");
+ armjoints.forearm = clonedRobot->getRobotNode("ArmL6_Elb2");
+ armjoints.wristAdduction = clonedRobot->getRobotNode("ArmL7_Wri1");
+ armjoints.wristExtension = clonedRobot->getRobotNode("ArmL8_Wri2");
+
+ VirtualRobot::SoechtingNullspaceGradientWithWristPtr gradient(
+ new VirtualRobot::SoechtingNullspaceGradientWithWrist(
+ target1, "ArmL2_Sho1", VirtualRobot::Soechting::ArmType::Left, armjoints));
gradient->kP = kSoechting;
ik->addNullspaceGradient(gradient);
}
- else std::cout << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node set for first robot node set for demonstration" << std::endl;
+ else
+ std::cout << "Soechting currently supports only Armar6 and RightArm/LeftArm robot node "
+ "set for first robot node set for demonstration"
+ << std::endl;
}
ui->solveButton->setEnabled(false);
ui->solveIKButton->setEnabled(false);
emit solveIKAsync(ik, untilReached ? -1 : ui->ikSteps->value(), nsman);
-
}
-void DiffIKWidget::setEndEffectorPose() {
- if (currentRobotNodeSet && currentRobotNodeSet->getTCP()) {
- auto tcp = currentRobotNodeSet->getTCP();
- auto pose = tcp->getPoseInRootFrame();
+void DiffIKWidget::setEndEffectorPose()
+{
+ if (currentRobotNodeSet && currentRobotNodeSet->getTCP())
+ {
+ auto tcp = currentRobotNodeSet->getTCP();
+ auto pose = tcp->getPoseInRootFrame();
auto position = simox::math::mat4f_to_pos(pose);
ui->xTarget->setValue(position(0));
ui->yTarget->setValue(position(1));
@@ -518,9 +698,10 @@ void DiffIKWidget::setEndEffectorPose() {
ui->pitchTarget->setValue(rpy(1));
ui->yawTarget->setValue(rpy(2));
}
- if (currentRobotNodeSet2 && currentRobotNodeSet2->getTCP()) {
- auto tcp = currentRobotNodeSet2->getTCP();
- auto pose = tcp->getPoseInRootFrame();
+ if (currentRobotNodeSet2 && currentRobotNodeSet2->getTCP())
+ {
+ auto tcp = currentRobotNodeSet2->getTCP();
+ auto pose = tcp->getPoseInRootFrame();
auto position = simox::math::mat4f_to_pos(pose);
ui->xTarget2->setValue(position(0));
ui->yTarget2->setValue(position(1));
@@ -532,15 +713,18 @@ void DiffIKWidget::setEndEffectorPose() {
}
}
-void DiffIKWidget::workerFinished() {
+void DiffIKWidget::workerFinished()
+{
ui->solveButton->setEnabled(true);
ui->solveIKButton->setEnabled(true);
emit currentManipUpdated();
}
-void DiffIKWidget::resetJointValues() {
+void DiffIKWidget::resetJointValues()
+{
auto values = robot->getJointValues();
- for (auto &value : values) {
+ for (auto& value : values)
+ {
value.second = 0;
}
robot->setJointValues(values);
@@ -548,9 +732,11 @@ void DiffIKWidget::resetJointValues() {
emit currentManipUpdated();
}
-void DiffIKWidget::setAverageJointValues() {
+void DiffIKWidget::setAverageJointValues()
+{
robot->setPropagatingJointValuesEnabled(false);
- for (auto node : robot->getRobotNodes()) {
+ for (auto node : robot->getRobotNodes())
+ {
if (node->isRotationalJoint())
robot->setJointValue(node, (node->getJointLimitLo() + node->getJointLimitHi()) / 2.0f);
}
@@ -560,23 +746,28 @@ void DiffIKWidget::setAverageJointValues() {
emit currentManipUpdated();
}
-float randomFloat(float a, float b) {
- float random = ((float) rand()) / (float) RAND_MAX;
- float diff = b - a;
- float r = random * diff;
+float randomFloat(float a, float b)
+{
+ float random = ((float)rand()) / (float)RAND_MAX;
+ float diff = b - a;
+ float r = random * diff;
return a + r;
}
-void DiffIKWidget::setRandomJointValues() {
+void DiffIKWidget::setRandomJointValues()
+{
robot->setPropagatingJointValuesEnabled(false);
std::vector<std::string> robotNodeNames = currentRobotNodeSet->getNodeNames();
- if (ui->checkBoxBimanual && currentRobotNodeSet2) {
+ if (ui->checkBoxBimanual && currentRobotNodeSet2)
+ {
auto robotNodeNames2 = currentRobotNodeSet2->getNodeNames();
robotNodeNames.insert(robotNodeNames.end(), robotNodeNames2.begin(), robotNodeNames2.end());
}
- for (const auto &name : robotNodeNames) {
+ for (const auto& name : robotNodeNames)
+ {
auto node = robot->getRobotNode(name);
- if (node->isRotationalJoint()) {
+ if (node->isRotationalJoint())
+ {
float jointValue = randomFloat(node->getJointLimitLo(), node->getJointLimitHi());
if (!std::isnan(jointValue))
robot->setJointValue(node, jointValue);
@@ -588,82 +779,110 @@ void DiffIKWidget::setRandomJointValues() {
emit currentManipUpdated();
}
-void DiffIKWidget::setRobotNodeSet(QString name) {
+void DiffIKWidget::setRobotNodeSet(QString name)
+{
currentRobotNodeSet = robot->getRobotNodeSet(name.toStdString());
}
-void DiffIKWidget::setRobotNodeSet2(QString name) {
+void DiffIKWidget::setRobotNodeSet2(QString name)
+{
currentRobotNodeSet2 = robot->getRobotNodeSet(name.toStdString());
}
-void printJacobi(VirtualRobot::RobotNodeSetPtr rns, bool scale) {
- if (!rns) return;
- VirtualRobot::DifferentialIKPtr diffIK(new VirtualRobot::DifferentialIK(rns, VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::eSVDDamped));
+void printJacobi(VirtualRobot::RobotNodeSetPtr rns, bool scale)
+{
+ if (!rns)
+ return;
+ VirtualRobot::DifferentialIKPtr diffIK(new VirtualRobot::DifferentialIK(
+ rns, VirtualRobot::RobotNodePtr(), VirtualRobot::JacobiProvider::eSVDDamped));
diffIK->convertModelScalingtoM(scale);
- Eigen::MatrixXd jacobian = diffIK->getJacobianMatrix(VirtualRobot::IKSolver::All).cast<double>();
- Eigen::IOFormat CommaInitFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";");
+ Eigen::MatrixXd jacobian =
+ diffIK->getJacobianMatrix(VirtualRobot::IKSolver::All).cast<double>();
+ Eigen::IOFormat CommaInitFmt(
+ Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", ", ", "", "", " << ", ";");
std::cout << "Jacobian matrix for " << rns->getName() << ":\n"
- << jacobian << "\n\n" << jacobian.format(CommaInitFmt) << "\n\n" << std::endl;
+ << jacobian << "\n\n"
+ << jacobian.format(CommaInitFmt) << "\n\n"
+ << std::endl;
}
-void DiffIKWidget::printJacobian() {
+void DiffIKWidget::printJacobian()
+{
printJacobi(currentRobotNodeSet, true);
}
-void DiffIKWidget::printJacobian2() {
+void DiffIKWidget::printJacobian2()
+{
printJacobi(currentRobotNodeSet, true);
}
-void Worker::followManip(VirtualRobot::AbstractManipulabilityTrackingPtr manipTracking, VirtualRobot::RobotNodeSetPtr rns,
- const Eigen::MatrixXd& followManip, float kGain, float maxDistance) {
- double distance = 1000;
+void Worker::followManip(VirtualRobot::AbstractManipulabilityTrackingPtr manipTracking,
+ VirtualRobot::RobotNodeSetPtr rns,
+ const Eigen::MatrixXd& followManip,
+ float kGain,
+ float maxDistance)
+{
+ double distance = 1000;
double lastDistance = 1000;
- int count = 0;
- while (distance > maxDistance) {
- Eigen::VectorXf velocity = manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
+ int count = 0;
+ while (distance > maxDistance)
+ {
+ Eigen::VectorXf velocity =
+ manipTracking->calculateVelocity(followManip, Eigen::MatrixXd(), true);
Eigen::VectorXf jointValues = rns->getJointValuesEigen() + velocity * kGain;
rns->setJointValues(jointValues);
distance = manipTracking->computeDistance(followManip);
emit distanceUpdated(distance);
- if (distance / lastDistance < 0.99) {
+ if (distance / lastDistance < 0.99)
+ {
emit jointValuesUpdated(rns->getJointValueMap());
emit currentManipUpdated();
lastDistance = distance;
- count = 0;
+ count = 0;
}
- else count++;
- if (count > 1000) break; // stop after 1000 iterations without large change
+ else
+ count++;
+ if (count > 1000)
+ break; // stop after 1000 iterations without large change
}
- if (distance < maxDistance) {
+ if (distance < maxDistance)
+ {
emit jointValuesUpdated(rns->getJointValueMap());
emit distanceUpdated(distance);
emit currentManipUpdated();
}
- else emit distanceUpdated(lastDistance);
+ else
+ emit distanceUpdated(lastDistance);
emit finished();
}
-void Worker::solveIK(VirtualRobot::CompositeDiffIKPtr ik, int steps, VirtualRobot::NullspaceManipulabilityPtr nsman) {
+void Worker::solveIK(VirtualRobot::CompositeDiffIKPtr ik,
+ int steps,
+ VirtualRobot::NullspaceManipulabilityPtr nsman)
+{
VirtualRobot::CompositeDiffIK::Parameters cp;
cp.resetRnsValues = false;
- cp.returnIKSteps = true;
- cp.steps = 1;
+ cp.returnIKSteps = true;
+ cp.steps = 1;
VirtualRobot::CompositeDiffIK::SolveState state;
ik->solve(cp, state);
int i = 0;
- while (i < steps || (steps < 0 && !ik->getLastResult().reached && i < 1000)) {
+ while (i < steps || (steps < 0 && !ik->getLastResult().reached && i < 1000))
+ {
ik->step(cp, state, i);
emit jointValuesUpdated(ik->getRobotNodeSet()->getJointValueMap());
- if (nsman) {
+ if (nsman)
+ {
emit distanceUpdated(nsman->computeDistance());
emit currentManipUpdated();
}
i++;
}
emit jointValuesUpdated(ik->getRobotNodeSet()->getJointValueMap());
- if (nsman) {
+ if (nsman)
+ {
emit distanceUpdated(nsman->computeDistance());
emit currentManipUpdated();
}
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index 7ecf31c79..169052551 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -222,7 +222,7 @@ void reachabilityWindow::reachVisu()
WorkspaceRepresentation::WorkspaceCut2DPtr cutData = reachSpace->createCut(pos,reachSpace->getDiscretizeParameterTranslation(), true);
VR_INFO << "Slider pos: " << pos << ", maxEntry:" << reachSpace->getMaxSummedAngleReachablity() << ", cut maxCoeff:" << cutData->entries.maxCoeff() << std::endl;
- SoNode *reachvisu2 = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), Eigen::Vector3f::UnitZ(), reachSpace->getMaxSummedAngleReachablity(), minAngle, maxAngle);
+ SoNode *reachvisu2 = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGray), Eigen::Vector3f::UnitZ(), reachSpace->getMaxSummedAngleReachablity(), minAngle, maxAngle);
visualisationNode->addChild(reachvisu2);
}
@@ -235,7 +235,7 @@ void reachabilityWindow::reachVisu()
reachSpace->getWorkspaceExtends(minBB, maxBB);
float zDist = maxBB(2) - minBB(2);
float maxZ = minBB(2) + heightPercent*zDist - reachSpace->getDiscretizeParameterTranslation();
- SoNode *reachvisu = CoinVisualizationFactory::getCoinVisualization(reachSpace, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), true, maxZ, minAngle, maxAngle);
+ SoNode *reachvisu = CoinVisualizationFactory::getCoinVisualization(reachSpace, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGray), true, maxZ, minAngle, maxAngle);
visualisationNode->addChild(reachvisu);
} else
@@ -621,9 +621,10 @@ void reachabilityWindow::createReach()
}
}
+ std::cout << "filafdasdfling holes of reachability space" << std::endl;
VR_INFO << "Adding random TCP poses for initialization...";
- reachSpace->addRandomTCPPoses(12'000'000);
+ reachSpace->addRandomTCPPoses(100'000, 12, false);
VR_INFO << "... done.";
reachSpace->print();
--
GitLab
From 6cd4f894850b40e72781dffab69b885c8afbd7af Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 7 Oct 2021 16:52:28 +0200
Subject: [PATCH 07/14] environment for first tests
---
VirtualRobot/examples/CMakeLists.txt | 1 +
.../examples/PlatformPlacement/CMakeLists.txt | 34 +
.../PlatformPlacement/ReachabilityMap.ui | 257 +++++
.../ReachabilityMapScene.cpp | 101 ++
.../ReachabilityMapWindow.cpp | 888 ++++++++++++++++++
.../PlatformPlacement/ReachabilityMapWindow.h | 103 ++
6 files changed, 1384 insertions(+)
create mode 100644 VirtualRobot/examples/PlatformPlacement/CMakeLists.txt
create mode 100644 VirtualRobot/examples/PlatformPlacement/ReachabilityMap.ui
create mode 100644 VirtualRobot/examples/PlatformPlacement/ReachabilityMapScene.cpp
create mode 100644 VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
create mode 100644 VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.h
diff --git a/VirtualRobot/examples/CMakeLists.txt b/VirtualRobot/examples/CMakeLists.txt
index c2181bb78..ddafba194 100644
--- a/VirtualRobot/examples/CMakeLists.txt
+++ b/VirtualRobot/examples/CMakeLists.txt
@@ -29,3 +29,4 @@ ADD_SUBDIRECTORY(CoinViewer)
ADD_SUBDIRECTORY(RGBOffscreenRendering)
ADD_SUBDIRECTORY(DepthOffscreenRendering)
ADD_SUBDIRECTORY(RobotFromObjects)
+ADD_SUBDIRECTORY(PlatformPlacement)
diff --git a/VirtualRobot/examples/PlatformPlacement/CMakeLists.txt b/VirtualRobot/examples/PlatformPlacement/CMakeLists.txt
new file mode 100644
index 000000000..2002aa6e4
--- /dev/null
+++ b/VirtualRobot/examples/PlatformPlacement/CMakeLists.txt
@@ -0,0 +1,34 @@
+PROJECT ( PlatformPlacement )
+
+IF(Simox_VISUALIZATION AND Simox_USE_COIN_VISUALIZATION)
+
+ # the variable "demo_SRCS" contains all .cpp files of this project
+ FILE(GLOB demo_SRCS ${PROJECT_SOURCE_DIR}/ReachabilityMapScene.cpp ${PROJECT_SOURCE_DIR}/ReachabilityMapWindow.cpp)
+ FILE(GLOB demo_INCS ${PROJECT_SOURCE_DIR}/ReachabilityMapWindow.h)
+
+ set(GUI_MOC_HDRS
+ ${PROJECT_SOURCE_DIR}/ReachabilityMapWindow.h
+ )
+ set(GUI_UIS
+ ${PROJECT_SOURCE_DIR}/ReachabilityMap.ui
+ )
+
+ # create the executable
+ VirtualRobotQtApplication(${PROJECT_NAME} "${demo_SRCS}" "${demo_INCS}" "${GUI_MOC_HDRS}" "${GUI_UIS}")
+ SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES RUNTIME_OUTPUT_DIRECTORY ${Simox_BIN_DIR})
+ SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES FOLDER "Examples")
+
+ #######################################################################################
+ ############################ Setup for installation ###################################
+ #######################################################################################
+
+ install(TARGETS ${PROJECT_NAME}
+ # IMPORTANT: Add the library to the "export-set"
+ EXPORT SimoxTargets
+ RUNTIME DESTINATION bin COMPONENT bin
+ COMPONENT dev)
+
+ MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be placed into " ${Simox_BIN_DIR})
+ MESSAGE( STATUS " ** Simox application ${PROJECT_NAME} will be installed into " bin)
+
+ENDIF()
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMap.ui b/VirtualRobot/examples/PlatformPlacement/ReachabilityMap.ui
new file mode 100644
index 000000000..bf453a5e0
--- /dev/null
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMap.ui
@@ -0,0 +1,257 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<ui version="4.0">
+ <class>MainWindowReachability</class>
+ <widget class="QMainWindow" name="MainWindowReachability">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>868</width>
+ <height>707</height>
+ </rect>
+ </property>
+ <property name="windowTitle">
+ <string> VirtualRobot - ReachabilityMap Demo</string>
+ </property>
+ <widget class="QWidget" name="centralwidget">
+ <layout class="QGridLayout" name="gridLayout">
+ <item row="0" column="0">
+ <widget class="QFrame" name="frameViewer">
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QGroupBox" name="groupBox">
+ <property name="maximumSize">
+ <size>
+ <width>200</width>
+ <height>16777215</height>
+ </size>
+ </property>
+ <property name="title">
+ <string/>
+ </property>
+ <widget class="QGroupBox" name="groupBox_3">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>460</y>
+ <width>201</width>
+ <height>271</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Visualization</string>
+ </property>
+ <widget class="QCheckBox" name="checkBoxRobot">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>70</y>
+ <width>131</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Robot</string>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxReachabilityMapVisu">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>110</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Reachability Map</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxObject">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>30</y>
+ <width>131</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Object</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxGrasps">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>50</y>
+ <width>131</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Grasp(s)</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QCheckBox" name="checkBoxReachabilityVisu">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>90</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Reachability Cut</string>
+ </property>
+ <property name="checked">
+ <bool>false</bool>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QGroupBox" name="groupBox_4">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>200</y>
+ <width>201</width>
+ <height>201</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Reachability Map</string>
+ </property>
+ <widget class="QRadioButton" name="radioButtonAllGrasps">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>120</y>
+ <width>161</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>All Grasps</string>
+ </property>
+ </widget>
+ <widget class="QRadioButton" name="radioButtonOneGrasp">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>140</y>
+ <width>131</width>
+ <height>17</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>One Grasp</string>
+ </property>
+ <property name="checked">
+ <bool>true</bool>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxGrasp">
+ <property name="geometry">
+ <rect>
+ <x>50</x>
+ <y>160</y>
+ <width>131</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QLabel" name="label">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>40</y>
+ <width>121</width>
+ <height>16</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>End Effector</string>
+ </property>
+ </widget>
+ <widget class="QComboBox" name="comboBoxEEF">
+ <property name="geometry">
+ <rect>
+ <x>18</x>
+ <y>60</y>
+ <width>151</width>
+ <height>22</height>
+ </rect>
+ </property>
+ </widget>
+ </widget>
+ <widget class="QGroupBox" name="groupBox_2">
+ <property name="geometry">
+ <rect>
+ <x>10</x>
+ <y>0</y>
+ <width>191</width>
+ <height>171</height>
+ </rect>
+ </property>
+ <property name="title">
+ <string>Object</string>
+ </property>
+ <widget class="QPushButton" name="pushButtonObjectRandom">
+ <property name="geometry">
+ <rect>
+ <x>20</x>
+ <y>30</y>
+ <width>161</width>
+ <height>31</height>
+ </rect>
+ </property>
+ <property name="text">
+ <string>Place Object Random</string>
+ </property>
+ </widget>
+ </widget>
+ </widget>
+ </item>
+ </layout>
+ </widget>
+ <widget class="QMenuBar" name="menubar">
+ <property name="geometry">
+ <rect>
+ <x>0</x>
+ <y>0</y>
+ <width>868</width>
+ <height>25</height>
+ </rect>
+ </property>
+ </widget>
+ <widget class="QStatusBar" name="statusbar"/>
+ </widget>
+ <tabstops>
+ <tabstop>radioButtonAllGrasps</tabstop>
+ <tabstop>radioButtonOneGrasp</tabstop>
+ <tabstop>comboBoxGrasp</tabstop>
+ <tabstop>checkBoxObject</tabstop>
+ <tabstop>checkBoxGrasps</tabstop>
+ <tabstop>checkBoxRobot</tabstop>
+ <tabstop>checkBoxReachabilityMapVisu</tabstop>
+ </tabstops>
+ <resources/>
+ <connections/>
+</ui>
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapScene.cpp b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapScene.cpp
new file mode 100644
index 000000000..92144de1c
--- /dev/null
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapScene.cpp
@@ -0,0 +1,101 @@
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/Qt/SoQt.h>
+
+#include <string>
+#include <iostream>
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#include "ReachabilityMapWindow.h"
+
+//#define ICUB
+
+using std::cout;
+using std::endl;
+using namespace VirtualRobot;
+
+int main(int argc, char* argv[])
+{
+ VirtualRobot::init(argc, argv, "Reachability Map Demo");
+
+ std::cout << " --- START --- " << std::endl;
+
+ std::string filenameReach;
+ std::string eef;
+#ifdef ICUB
+ std::string filenameRob("robots/iCub/iCub.xml");
+ std::string fileObj("objects/iCub/LegoXWing_RightHand_300.xml");
+ filenameReach = "reachability/iCub_HipLeftArm.bin";
+#else
+ std::string filenameRob("/home/fabi/workspace/armarx_integration/robots/armar6/rt/data/armar6_rt/robotmodel/Armar6-SH/Armar6-SH.xml");
+ std::string fileObj("objects/ArmarIII/WaterBottle_RightHand_1000.xml");
+ filenameReach = "reachability/ArmarIII_PlatformHipRightArm.bin";
+ eef = "Hand R";
+#endif
+
+
+ VirtualRobot::RuntimeEnvironment::considerKey("robot");
+ VirtualRobot::RuntimeEnvironment::considerKey("eef");
+ VirtualRobot::RuntimeEnvironment::considerKey("reachability");
+ VirtualRobot::RuntimeEnvironment::considerKey("object");
+ VirtualRobot::RuntimeEnvironment::processCommandLine(argc, argv);
+ VirtualRobot::RuntimeEnvironment::print();
+
+ std::cout << " --- START --- " << std::endl;
+
+ if (VirtualRobot::RuntimeEnvironment::hasValue("robot"))
+ {
+ std::string robFile = VirtualRobot::RuntimeEnvironment::getValue("robot");
+
+ if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robFile))
+ {
+ filenameRob = robFile;
+ }
+ }
+
+ if (VirtualRobot::RuntimeEnvironment::hasValue("reachability"))
+ {
+ std::string reachFile = VirtualRobot::RuntimeEnvironment::getValue("reachability");
+
+ if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(reachFile))
+ {
+ filenameReach = reachFile;
+ }
+ }
+
+ if (VirtualRobot::RuntimeEnvironment::hasValue("object"))
+ {
+ std::string of = VirtualRobot::RuntimeEnvironment::getValue("object");
+
+ if (VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(of))
+ {
+ fileObj = of;
+ }
+ }
+
+ if (VirtualRobot::RuntimeEnvironment::hasValue("eef"))
+ {
+ eef = VirtualRobot::RuntimeEnvironment::getValue("eef");
+ }
+
+ std::cout << "Using robot at " << filenameRob << std::endl;
+ std::cout << "Using eef " << eef << std::endl;
+ std::cout << "Using manipulation object at " << fileObj << std::endl;
+ std::cout << "Using reachability file from " << filenameReach << std::endl;
+
+ ReachabilityMapWindow rw(filenameRob, filenameReach, fileObj, eef);
+
+ rw.main();
+
+ return 0;
+
+}
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
new file mode 100644
index 000000000..1a45e35b3
--- /dev/null
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
@@ -0,0 +1,888 @@
+
+#include "ReachabilityMapWindow.h"
+#include <VirtualRobot/EndEffector/EndEffector.h>
+#include <VirtualRobot/XML/RobotIO.h>
+#include <VirtualRobot/XML/ObjectIO.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
+#include <VirtualRobot/RuntimeEnvironment.h>
+#include <VirtualRobot/Workspace/Reachability.h>
+#include <VirtualRobot/Workspace/Manipulability.h>
+#include <VirtualRobot/Workspace/WorkspaceGrid.h>
+#include <QFileDialog>
+#include <Eigen/Geometry>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Transform.h>
+#include <Eigen/src/Geometry/Translation.h>
+#include <cstdlib>
+#include <ctime>
+#include <vector>
+#include <iostream>
+#include <cmath>
+
+#include "Inventor/actions/SoLineHighlightRenderAction.h"
+#include "VirtualRobot/ManipulationObject.h"
+#include "VirtualRobot/MathTools.h"
+#include "VirtualRobot/Obstacle.h"
+#include "VirtualRobot/VirtualRobot.h"
+#include <Inventor/nodes/SoShapeHints.h>
+#include <Inventor/nodes/SoLightModel.h>
+
+
+#include <sstream>
+using namespace std;
+using namespace VirtualRobot;
+
+float TIMER_MS = 30.0f;
+
+//#define ENDLESS
+
+ReachabilityMapWindow::ReachabilityMapWindow(std::string& sRobotFile, std::string& reachFile, std::string& objFile, std::string& eef)
+ : QMainWindow(nullptr)
+{
+ VR_INFO << " start " << std::endl;
+
+ robotFile = sRobotFile;
+ VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(robotFile);
+ sceneSep = new SoSeparator;
+ sceneSep->ref();
+ robotVisuSep = new SoSeparator;
+ robotVisuSep->ref();
+ reachabilityVisuSep = new SoSeparator;
+ reachabilityVisuSep->ref();
+ reachabilityMapVisuSep = new SoSeparator;
+ reachabilityMapVisuSep->ref();
+ allGraspsVisuSep = new SoSeparator;
+ allGraspsVisuSep->ref();
+ graspVisuSep = new SoSeparator;
+ graspVisuSep->ref();
+ objectVisuSep = new SoSeparator;
+ objectVisuSep->ref();
+
+ sceneSep->addChild(objectVisuSep);
+ sceneSep->addChild(graspVisuSep);
+ sceneSep->addChild(reachabilityVisuSep);
+ sceneSep->addChild(reachabilityMapVisuSep);
+
+ setupUI();
+
+ loadRobot();
+
+ if (!reachFile.empty())
+ {
+ if (RuntimeEnvironment::getDataFileAbsolute(reachFile))
+ {
+ loadReachFile(reachFile);
+ }
+ }
+
+ if (!objFile.empty())
+ {
+ if (RuntimeEnvironment::getDataFileAbsolute(objFile))
+ {
+ loadObjectFile(objFile);
+ }
+ }
+
+ setupEnvironment();
+ updateVisu();
+
+ if (!eef.empty())
+ {
+ selectEEF(eef);
+ }
+
+ viewer->viewAll();
+}
+
+
+ReachabilityMapWindow::~ReachabilityMapWindow()
+{
+ robotVisuSep->unref();
+ reachabilityVisuSep->unref();
+ reachabilityMapVisuSep->unref();
+ allGraspsVisuSep->unref();
+ graspVisuSep->unref();
+ objectVisuSep->unref();
+ sceneSep->unref();
+}
+
+
+void ReachabilityMapWindow::setupUI()
+{
+ UI.setupUi(this);
+ viewer = new SoQtExaminerViewer(UI.frameViewer, "", TRUE, SoQtExaminerViewer::BUILD_POPUP);
+
+ // setup
+ viewer->setBackgroundColor(SbColor(1.0f, 1.0f, 1.0f));
+ viewer->setAccumulationBuffer(false);
+
+ viewer->setAntialiasing(true, 4);
+
+ viewer->setGLRenderAction(new SoLineHighlightRenderAction);
+ viewer->setTransparencyType(SoGLRenderAction::BLEND);
+ viewer->setFeedbackVisibility(true);
+ viewer->setSceneGraph(sceneSep);
+ viewer->viewAll();
+
+ connect(UI.pushButtonObjectRandom, SIGNAL(clicked()), this, SLOT(setObjectRandom()));
+
+ connect(UI.checkBoxRobot, SIGNAL(clicked()), this, SLOT(updateVisu()));
+ connect(UI.checkBoxGrasps, SIGNAL(clicked()), this, SLOT(updateVisu()));
+ connect(UI.checkBoxObject, SIGNAL(clicked()), this, SLOT(updateVisu()));
+ connect(UI.checkBoxReachabilityVisu, SIGNAL(clicked()), this, SLOT(updateVisu()));
+ connect(UI.checkBoxReachabilityMapVisu, SIGNAL(clicked()), this, SLOT(updateVisu()));
+
+ connect(UI.radioButtonAllGrasps, SIGNAL(clicked()), this, SLOT(selectGrasp()));
+ connect(UI.radioButtonOneGrasp, SIGNAL(clicked()), this, SLOT(selectGrasp()));
+ connect(UI.comboBoxGrasp, SIGNAL(currentIndexChanged(int)), this, SLOT(selectGrasp()));
+ connect(UI.comboBoxEEF, SIGNAL(currentIndexChanged(int)), this, SLOT(selectEEF()));
+
+}
+
+QString ReachabilityMapWindow::formatString(const char* s, float f)
+{
+ QString str1(s);
+
+ if (f >= 0)
+ {
+ str1 += " ";
+ }
+
+ if (fabs(f) < 1000)
+ {
+ str1 += " ";
+ }
+
+ if (fabs(f) < 100)
+ {
+ str1 += " ";
+ }
+
+ if (fabs(f) < 10)
+ {
+ str1 += " ";
+ }
+
+ QString str1n;
+ str1n.setNum(f, 'f', 3);
+ str1 = str1 + str1n;
+ return str1;
+}
+
+
+void ReachabilityMapWindow::resetSceneryAll()
+{
+ if (!robot)
+ {
+ return;
+ }
+
+ std::vector< RobotNodePtr > nodes;
+ robot->getRobotNodes(nodes);
+ std::vector<float> jv(nodes.size(), 0.0f);
+ robot->setJointValues(nodes, jv);
+}
+
+void ReachabilityMapWindow::updateVisu()
+{
+ if (UI.checkBoxRobot->isChecked())
+ {
+ if (robotVisuSep->getNumChildren() == 0)
+ {
+ buildRobotVisu();
+ }
+
+ if (sceneSep->findChild(robotVisuSep) < 0)
+ {
+ sceneSep->addChild(robotVisuSep);
+ }
+ }
+ else
+ {
+ if (sceneSep->findChild(robotVisuSep) >= 0)
+ {
+ sceneSep->removeChild(robotVisuSep);
+ }
+ }
+
+ if (UI.checkBoxGrasps->isChecked())
+ {
+ if (graspVisuSep->getNumChildren() == 0)
+ {
+ buildGraspVisu();
+ }
+
+ if (sceneSep->findChild(graspVisuSep) < 0)
+ {
+ sceneSep->addChild(graspVisuSep);
+ }
+ }
+ else
+ {
+ if (sceneSep->findChild(graspVisuSep) >= 0)
+ {
+ sceneSep->removeChild(graspVisuSep);
+ }
+ }
+
+ if (UI.checkBoxObject->isChecked())
+ {
+ if (objectVisuSep->getNumChildren() == 0)
+ {
+ buildObjectVisu();
+ }
+
+ if (sceneSep->findChild(objectVisuSep) < 0)
+ {
+ sceneSep->addChild(objectVisuSep);
+ }
+ }
+ else
+ {
+ if (sceneSep->findChild(objectVisuSep) >= 0)
+ {
+ sceneSep->removeChild(objectVisuSep);
+ }
+ }
+
+ if (UI.checkBoxReachabilityVisu->isChecked())
+ {
+ if (reachabilityVisuSep->getNumChildren() == 0)
+ {
+ buildReachVisu();
+ }
+
+ if (sceneSep->findChild(reachabilityVisuSep) < 0)
+ {
+ sceneSep->addChild(reachabilityVisuSep);
+ }
+ }
+ else
+ {
+ if (sceneSep->findChild(reachabilityVisuSep) >= 0)
+ {
+ sceneSep->removeChild(reachabilityVisuSep);
+ }
+ }
+
+ if (UI.checkBoxReachabilityMapVisu->isChecked())
+ {
+ if (reachabilityMapVisuSep->getNumChildren() == 0)
+ {
+ buildReachGridVisu();
+ }
+
+ if (sceneSep->findChild(reachabilityMapVisuSep) < 0)
+ {
+ sceneSep->addChild(reachabilityMapVisuSep);
+ }
+ }
+ else
+ {
+ if (sceneSep->findChild(reachabilityMapVisuSep) >= 0)
+ {
+ sceneSep->removeChild(reachabilityMapVisuSep);
+ }
+ }
+}
+
+
+void ReachabilityMapWindow::buildReachVisu()
+{
+ if (!robot || !reachSpace || !graspObject || !eef)
+ {
+ return;
+ }
+
+
+ reachabilityVisuSep->removeAllChildren();
+
+ GraspSetPtr gs = graspObject->getGraspSet(eef);
+
+ if (!gs || gs->getSize() == 0)
+ {
+ return;
+ }
+
+ Eigen::Matrix4f pose = graspObject->getGlobalPose();
+
+ if (UI.radioButtonOneGrasp->isChecked())
+ {
+ QString qs(UI.comboBoxGrasp->currentText());
+ std::string s(qs.toLatin1());
+ GraspPtr g = gs->getGrasp(s);
+
+ if (g)
+ {
+ pose = g->getTcpPoseGlobal(graspObject->getGlobalPose());
+ }
+ }
+
+ WorkspaceRepresentation::WorkspaceCut2DPtr cutData = reachSpace->createCut(pose,reachSpace->getDiscretizeParameterTranslation(), false);
+ int maxCoeff = cutData->entries.maxCoeff();
+ VR_INFO << "Max coeff:" << maxCoeff << std::endl;
+
+ SoNode *visualisationNode = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), Eigen::Vector3f::UnitZ(), maxCoeff);
+
+ if (visualisationNode)
+ {
+ if (reachSpace->getBaseNode())
+ {
+ Eigen::Matrix4f gp = reachSpace->getBaseNode()->getGlobalPose();
+ reachabilityVisuSep->addChild(CoinVisualizationFactory::getMatrixTransform(gp));
+ }
+ reachabilityVisuSep->addChild(visualisationNode);
+ }
+
+}
+
+void ReachabilityMapWindow::buildRobotVisu()
+{
+ robotVisuSep->removeAllChildren();
+
+ if (!robot)
+ {
+ return;
+ }
+
+ std::shared_ptr<VirtualRobot::CoinVisualization> visualization = robot->getVisualization<CoinVisualization>();
+ SoNode* visualisationNode = nullptr;
+
+ if (visualization)
+ {
+ visualisationNode = visualization->getCoinVisualization();
+ }
+
+ if (visualisationNode)
+ {
+ robotVisuSep->addChild(visualisationNode);
+ }
+}
+
+void ReachabilityMapWindow::buildObjectVisu()
+{
+ objectVisuSep->removeAllChildren();
+
+ if (!graspObject)
+ {
+ return;
+ }
+
+ std::shared_ptr<VirtualRobot::CoinVisualization> visualization = graspObject->getVisualization<CoinVisualization>();
+ SoNode* visualisationNode = nullptr;
+
+ if (visualization)
+ {
+ visualisationNode = visualization->getCoinVisualization();
+ }
+
+ if (visualisationNode)
+ {
+ objectVisuSep->addChild(visualisationNode);
+ }
+
+ if (environment)
+ {
+ visualization = environment->getVisualization<CoinVisualization>();
+
+ if (visualization)
+ {
+ visualisationNode = visualization->getCoinVisualization();
+ }
+
+ if (visualisationNode)
+ {
+ objectVisuSep->addChild(visualisationNode);
+ }
+ }
+}
+
+void ReachabilityMapWindow::buildGraspVisu()
+{
+ graspVisuSep->removeAllChildren();
+
+ if (!robot || !graspObject || !eef)
+ {
+ return;
+ }
+
+ GraspSetPtr gs = graspObject->getGraspSet(eef);
+
+ if (!gs || gs->getSize() == 0)
+ {
+ return;
+ }
+
+ if (UI.radioButtonOneGrasp->isChecked())
+ {
+ QString qs(UI.comboBoxGrasp->currentText());
+ std::string s(qs.toLatin1());
+ GraspPtr g = gs->getGrasp(s);
+
+ if (!g)
+ {
+ return;
+ }
+
+ SoSeparator* v = CoinVisualizationFactory::CreateGraspVisualization(g, eef, graspObject->getGlobalPose());
+
+ if (v)
+ {
+ graspVisuSep->addChild(v);
+ }
+ }
+ else
+ {
+ SoSeparator* v = CoinVisualizationFactory::CreateGraspSetVisualization(gs, eef, graspObject->getGlobalPose());
+
+ if (v)
+ {
+ graspVisuSep->addChild(v);
+ }
+ }
+}
+
+void ReachabilityMapWindow::buildReachGridVisu()
+{
+ if (!robot || !reachGrid)
+ {
+ return;
+ }
+
+ reachabilityMapVisuSep->removeAllChildren();
+
+ SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachGrid, VirtualRobot::ColorMap::eHot, true);
+
+ if (visualisationNode)
+ {
+ reachabilityMapVisuSep->addChild(visualisationNode);
+ }
+}
+
+void ReachabilityMapWindow::closeEvent(QCloseEvent* event)
+{
+ quit();
+ QMainWindow::closeEvent(event);
+}
+
+
+int ReachabilityMapWindow::main()
+{
+ SoQt::show(this);
+ SoQt::mainLoop();
+ return 0;
+}
+
+
+void ReachabilityMapWindow::quit()
+{
+ std::cout << "ReachabilityMapWindow: Closing" << std::endl;
+ this->close();
+ SoQt::exitMainLoop();
+}
+
+void ReachabilityMapWindow::updateEEFBox()
+{
+ UI.comboBoxEEF->clear();
+
+ if (!robot)
+ {
+ selectGrasp();
+ return;
+ }
+
+ std::vector<EndEffectorPtr> eefs = robot->getEndEffectors();
+
+ for (auto & eef : eefs)
+ {
+ UI.comboBoxEEF->addItem(QString(eef->getName().c_str()));
+ }
+
+ selectEEF(0);
+}
+
+void ReachabilityMapWindow::selectGrasp()
+{
+ if (!grasps)
+ {
+ return;
+ }
+
+ graspVisuSep->removeAllChildren();
+ reachabilityMapVisuSep->removeAllChildren();
+ reachabilityVisuSep->removeAllChildren();
+
+ if (UI.radioButtonAllGrasps->isChecked())
+ {
+ buildReachMapAll();
+ }
+ else
+ {
+ GraspPtr g = grasps->getGrasp(UI.comboBoxGrasp->currentIndex());
+ buildReachMap(g);
+ }
+
+ updateVisu();
+}
+
+
+void ReachabilityMapWindow::selectEEF()
+{
+ selectEEF(UI.comboBoxEEF->currentIndex());
+}
+
+void ReachabilityMapWindow::selectEEF(int nr)
+{
+ eef.reset();
+ grasps.reset();
+ UI.comboBoxGrasp->clear();
+ graspVisuSep->removeAllChildren();
+
+ if (!robot)
+ {
+ selectGrasp();
+ return;
+ }
+
+ std::cout << "Selecting EEF nr " << nr << std::endl;
+
+ std::vector<EndEffectorPtr> eefs = robot->getEndEffectors();
+ std::string tcp = "<not set>";
+
+ if (nr < 0 || nr >= (int)eefs.size())
+ {
+ return;
+ }
+
+ eef = eefs[nr];
+
+ if (graspObject)
+ {
+ grasps = graspObject->getGraspSet(eef);
+
+ if (grasps)
+ {
+ for (unsigned int i = 0; i < grasps->getSize(); i++)
+ {
+ UI.comboBoxGrasp->addItem(QString(grasps->getGrasp(i)->getName().c_str()));
+ }
+ }
+ }
+
+ selectGrasp();
+}
+
+void ReachabilityMapWindow::selectEEF(std::string& eef)
+{
+ if (!robot)
+ {
+ return;
+ }
+
+ std::vector<EndEffectorPtr> eefs = robot->getEndEffectors();
+
+ for (size_t i = 0; i < eefs.size(); i++)
+ {
+ if (eefs[i]->getName() == eef)
+ {
+ selectEEF((int)i);
+ UI.comboBoxEEF->setCurrentIndex((int)i);
+ }
+ }
+}
+
+void ReachabilityMapWindow::loadRobot()
+{
+ robotVisuSep->removeAllChildren();
+ std::cout << "Loading robot from " << robotFile << std::endl;
+
+ try
+ {
+ robot = RobotIO::loadRobot(robotFile);
+ }
+ catch (VirtualRobotException& e)
+ {
+ std::cout << " ERROR while creating robot" << std::endl;
+ std::cout << e.what();
+ return;
+ }
+
+ if (!robot)
+ {
+ std::cout << " ERROR while creating robot" << std::endl;
+ return;
+ }
+
+ //Eigen::Matrix4f gp = MathTools::rpy2eigen4f(0,0,M_PI/2.0);
+ Eigen::Matrix4f gp = Eigen::Matrix4f::Identity();
+ //gp(0,3) = 3000;
+ robot->setGlobalPose(gp);
+
+ updateEEFBox();
+
+ // build visualization
+ updateVisu();
+ viewer->viewAll();
+}
+
+void ReachabilityMapWindow::loadReachFile(std::string filename)
+{
+ if (!robot)
+ {
+ return;
+ }
+
+ return;
+
+ reachFile = filename;
+ bool success = false;
+
+ // 1st try to load as manipulability file
+ try
+ {
+ reachSpace.reset(new Manipulability(robot));
+ reachSpace->load(reachFile);
+ success = true;
+
+ VR_INFO << "Map '" << reachFile << "' loaded as Manipulability map";
+ }
+ catch (...)
+ {
+ }
+
+ // 2nd try to load as reachability file
+ if (!success)
+ {
+ try
+ {
+ reachSpace.reset(new Reachability(robot));
+ reachSpace->load(reachFile);
+ success = true;
+
+ VR_INFO << "Map '" << reachFile << "' loaded as Reachability map";
+ }
+ catch (...)
+ {
+ VR_ERROR << "Coulkd not load reachability file..." << std::endl;
+ }
+ }
+
+
+ reachSpace->print();
+ /*if (reachSpace->getNodeSet())
+ {
+ std::cout << "Using RNS: " << reachSpace->getNodeSet()->getName() << std::endl;
+ for (size_t i=0;i<robotNodeSets.size();i++)
+ {
+ std::cout << "checking " << robotNodeSets[i]->getName() << std::endl;
+ if (robotNodeSets[i] == reachSpace->getNodeSet())
+ {
+ std::cout << "Found RNS.." << std::endl;
+ //selectRNS(i);
+ }
+ }
+ }*/
+}
+void ReachabilityMapWindow::setObjectRandom()
+{
+ constexpr int margin = 100;
+ constexpr int height = 100;
+
+ constexpr int tableSizeX = 1200 * 1.1; // scale factors in the xml file
+ constexpr int tableSizeY = 800 * 1.1;
+
+ constexpr int tableSampleSizeX = tableSizeX - 2*margin;
+ constexpr int tableSampleSizeY = tableSizeY - 2*margin;
+
+ if (graspObject)
+ {
+ Eigen::Vector3f position = Eigen::Vector3f::Zero();
+ const float yaw = MathTools::deg2rad(std::rand() % 360);
+
+
+ enum class Side : int
+ {
+ LEFT,
+ FRONT,
+ RIGHT,
+ BACK
+ };
+
+ const Side sampledSide = static_cast<Side>(std::rand() % 4);
+
+ const auto sampleX = margin + static_cast<float>(rand() % tableSampleSizeX);
+ const auto sampleY = -(margin + static_cast<float>(rand() % tableSampleSizeY));
+
+ switch(sampledSide)
+ {
+ case Side::LEFT:
+ position.x() = margin;
+ position.y() = sampleY;
+ break;
+ case Side::FRONT:
+ position.x() = sampleX;
+ position.y() = -(tableSizeY - margin);
+ break;
+ case Side::RIGHT:
+ position.x() = tableSizeX - margin;
+ position.y() = sampleY;
+ break;
+ case Side::BACK:
+ position.x() = sampleX;
+ position.y() = -margin;
+ break;
+ }
+
+ // position.x() = tableSizeX;
+ // position.y() = -tableSizeY;
+
+ position.z() = 700.F * 1.2 + height;
+
+ VR_INFO << "Sampling position " << position;
+
+ Eigen::Isometry3f globalPose = Eigen::Isometry3f::Identity();
+ globalPose.translation() = position;
+ globalPose.linear() = Eigen::AngleAxisf(yaw, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+
+ graspObject->setGlobalPose(globalPose.matrix());
+ selectGrasp();
+ }
+}
+void ReachabilityMapWindow::setupEnvironment()
+{
+ std::string objectFile("objects/Table.xml");
+
+ if (!RuntimeEnvironment::getDataFileAbsolute(objectFile))
+ {
+ VR_ERROR << "No path to " << objectFile << std::endl;
+ return;
+ }
+
+ try
+ {
+ environment = ObjectIO::loadManipulationObject(objectFile);
+ }
+ catch(const VirtualRobotException &e)
+ {
+ VR_ERROR << "Could not load " << objectFile << std::endl;
+ return;
+ }
+
+ if (!environment)
+ {
+ return;
+ }
+
+ Eigen::Matrix4f gp;
+ gp.setIdentity();
+ environment->setGlobalPose(gp);
+ setObjectRandom();
+
+}
+
+void ReachabilityMapWindow::loadObjectFile(std::string filename)
+{
+ if (!robot)
+ {
+ return;
+ }
+
+ objectFile = filename;
+
+
+ try
+ {
+ const auto sphere = VirtualRobot::Obstacle::createSphere(50.F);
+ graspObject = std::make_shared<VirtualRobot::ManipulationObject>("target", sphere->getVisualization());
+
+ {
+ const std::string eef = "Hand_R_EEF";
+
+ GraspSetPtr graspSet = std::make_shared<GraspSet>("all", robot->getType(), eef);
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+ pose.linear() = Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+
+ GraspPtr grasp = std::make_shared<Grasp>("frontal", robot->getType(), eef, pose.matrix());
+ graspSet->addGrasp(grasp);
+ graspObject->addGraspSet(graspSet);
+ }
+
+ {
+ const std::string eef = "Hand_L_EEF";
+
+ GraspSetPtr graspSet = std::make_shared<GraspSet>("all", robot->getType(), eef);
+ Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
+ pose.linear() = Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix()* Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()).toRotationMatrix() ;
+ GraspPtr grasp = std::make_shared<Grasp>("frontal", robot->getType(), eef, pose.matrix());
+ graspSet->addGrasp(grasp);
+ graspObject->addGraspSet(graspSet);
+ }
+
+
+ for(const auto& graspSet : graspObject->getAllGraspSets())
+ {
+ VR_INFO << "Name: " << graspSet->getName();
+ VR_INFO << "Type: " << graspSet->getRobotType();
+ VR_INFO << "EEF: " << graspSet->getEndEffector();
+ }
+ }
+ catch(const VirtualRobotException &e)
+ {
+ VR_ERROR << "Could not load " << filename << std::endl;
+ return;
+ }
+}
+
+bool ReachabilityMapWindow::buildReachMapAll()
+{
+ reachabilityMapVisuSep->removeAllChildren();
+
+ if (!grasps)
+ {
+ return false;
+ }
+
+ if(not reachSpace)
+ {
+ return false;
+ }
+
+ Eigen::Vector3f minBB, maxBB;
+ reachSpace->getWorkspaceExtends(minBB, maxBB);
+ reachGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), reachSpace->getDiscretizeParameterTranslation()));
+
+ Eigen::Matrix4f gp = graspObject->getGlobalPose();
+ reachGrid->setGridPosition(gp(0, 3), gp(1, 3));
+
+ for (int i = 0; i < (int)grasps->getSize(); i++)
+ {
+ GraspPtr g = grasps->getGrasp(i);
+ reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode());
+ }
+
+ updateVisu();
+ return true;
+}
+
+bool ReachabilityMapWindow::buildReachMap(VirtualRobot::GraspPtr g)
+{
+ reachabilityMapVisuSep->removeAllChildren();
+ Eigen::Vector3f minBB, maxBB;
+
+ if(not reachSpace)
+ {
+ return false;
+ }
+
+ reachSpace->getWorkspaceExtends(minBB, maxBB);
+ reachGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), reachSpace->getDiscretizeParameterTranslation()));
+
+ Eigen::Matrix4f gp = graspObject->getGlobalPose();
+ reachGrid->setGridPosition(gp(0, 3), gp(1, 3));
+
+ reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode());
+
+ updateVisu();
+ return true;
+
+}
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.h b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.h
new file mode 100644
index 000000000..ef257e312
--- /dev/null
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.h
@@ -0,0 +1,103 @@
+
+#pragma once
+
+#include <VirtualRobot/VirtualRobot.h>
+#include <VirtualRobot/Robot.h>
+#include <VirtualRobot/VirtualRobotException.h>
+#include <VirtualRobot/Nodes/RobotNode.h>
+#include <VirtualRobot/XML/SceneIO.h>
+#include <VirtualRobot/Visualization/VisualizationFactory.h>
+#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualization.h>
+#include <VirtualRobot/Obstacle.h>
+#include <string.h>
+#include <QtCore/QtGlobal>
+#include <QtGui/QtGui>
+#include <QtCore/QtCore>
+#include <VirtualRobot/Grasping/Grasp.h>
+#include <VirtualRobot/Grasping/GraspSet.h>
+
+#include <Inventor/sensors/SoTimerSensor.h>
+#include <Inventor/nodes/SoEventCallback.h>
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/nodes/SoSeparator.h>
+
+
+#include <vector>
+
+#include "ui_ReachabilityMap.h"
+
+class ReachabilityMapWindow : public QMainWindow
+{
+ Q_OBJECT
+public:
+ ReachabilityMapWindow(std::string& sRobotFile, std::string& reachFile, std::string& objFile, std::string& eef);
+ ~ReachabilityMapWindow() override;
+
+ /*!< Executes the SoQt mainLoop. You need to call this in order to execute the application. */
+ int main();
+
+public slots:
+ /*! Closes the window and exits SoQt runloop. */
+ void quit();
+
+ /*!< Overriding the close event, so we know when the window was closed by the user. */
+ void closeEvent(QCloseEvent* event) override;
+ void resetSceneryAll();
+
+ void updateVisu();
+ void selectEEF();
+ void selectGrasp();
+ void setObjectRandom();
+
+protected:
+ void selectEEF(std::string& eef);
+ void selectEEF(int nr);
+ void loadRobot();
+ void updateEEFBox();
+ void buildRobotVisu();
+ void buildObjectVisu();
+ void buildReachVisu();
+ void buildReachGridVisu();
+ void buildGraspVisu();
+
+ bool buildReachMapAll();
+ bool buildReachMap(VirtualRobot::GraspPtr g);
+
+ void setupUI();
+ QString formatString(const char* s, float f);
+
+ void loadReachFile(std::string filename);
+ void loadObjectFile(std::string filename);
+ void setupEnvironment();
+
+ Ui::MainWindowReachability UI;
+ SoQtExaminerViewer* viewer; /*!< Viewer to display the 3D model of the robot and the environment. */
+
+ SoSeparator* sceneSep;
+ SoSeparator* robotVisuSep;
+ SoSeparator* reachabilityVisuSep;
+ SoSeparator* reachabilityMapVisuSep;
+ SoSeparator* allGraspsVisuSep;
+ SoSeparator* graspVisuSep;
+ SoSeparator* objectVisuSep;
+
+ VirtualRobot::RobotPtr robot;
+ VirtualRobot::EndEffectorPtr eef;
+ VirtualRobot::GraspSetPtr grasps;
+ VirtualRobot::ManipulationObjectPtr graspObject;
+ VirtualRobot::ManipulationObjectPtr environment;
+ std::string robotFile;
+ std::string reachFile;
+ std::string objectFile;
+ VirtualRobot::RobotNodeSetPtr currentRobotNodeSet;
+ std::vector < VirtualRobot::RobotNodePtr > allRobotNodes;
+ std::vector < VirtualRobot::RobotNodePtr > currentRobotNodes;
+ std::vector < VirtualRobot::RobotNodeSetPtr > robotNodeSets;
+
+ VirtualRobot::WorkspaceRepresentationPtr reachSpace;
+ VirtualRobot::WorkspaceGridPtr reachGrid;
+ VirtualRobot::RobotNodePtr currentRobotNode;
+
+};
+
--
GitLab
From c3dd2571d3c0c0b48d15b9a08d456f1b3a0bb265 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Thu, 7 Oct 2021 17:21:45 +0200
Subject: [PATCH 08/14] pre pose
---
.../ReachabilityMapWindow.cpp | 27 ++++++++++++++-----
1 file changed, 21 insertions(+), 6 deletions(-)
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
index 1a45e35b3..5e08820bc 100644
--- a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
@@ -632,8 +632,6 @@ void ReachabilityMapWindow::loadReachFile(std::string filename)
return;
}
- return;
-
reachFile = filename;
bool success = false;
@@ -789,6 +787,8 @@ void ReachabilityMapWindow::loadObjectFile(std::string filename)
objectFile = filename;
+ const float prePoseOffsetX = 300;
+ const float orientationOffset = -1;
try
{
@@ -800,10 +800,15 @@ void ReachabilityMapWindow::loadObjectFile(std::string filename)
GraspSetPtr graspSet = std::make_shared<GraspSet>("all", robot->getType(), eef);
Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
- pose.linear() = Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
+ pose.linear() = Eigen::AngleAxisf(M_PI + orientationOffset, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix();
- GraspPtr grasp = std::make_shared<Grasp>("frontal", robot->getType(), eef, pose.matrix());
+ GraspPtr grasp = std::make_shared<Grasp>("grasp", robot->getType(), eef, pose.matrix());
graspSet->addGrasp(grasp);
+
+ Eigen::Isometry3f prePose = pose * Eigen::Translation3f(Eigen::Vector3f{prePoseOffsetX,0,0});
+ GraspPtr preGrasp = std::make_shared<Grasp>("pre", robot->getType(), eef, prePose.matrix());
+ graspSet->addGrasp(preGrasp);
+
graspObject->addGraspSet(graspSet);
}
@@ -812,9 +817,15 @@ void ReachabilityMapWindow::loadObjectFile(std::string filename)
GraspSetPtr graspSet = std::make_shared<GraspSet>("all", robot->getType(), eef);
Eigen::Isometry3f pose = Eigen::Isometry3f::Identity();
- pose.linear() = Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix()* Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()).toRotationMatrix() ;
- GraspPtr grasp = std::make_shared<Grasp>("frontal", robot->getType(), eef, pose.matrix());
+ pose.linear() = Eigen::AngleAxisf(M_PI - orientationOffset, Eigen::Vector3f::UnitZ()).toRotationMatrix()*Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitY()).toRotationMatrix()* Eigen::AngleAxisf(M_PI, Eigen::Vector3f::UnitX()).toRotationMatrix() ;
+ GraspPtr grasp = std::make_shared<Grasp>("grasp", robot->getType(), eef, pose.matrix());
graspSet->addGrasp(grasp);
+
+
+ Eigen::Isometry3f prePose = pose * Eigen::Translation3f(Eigen::Vector3f{prePoseOffsetX,0,0});
+ GraspPtr preGrasp = std::make_shared<Grasp>("pre", robot->getType(), eef, prePose.matrix());
+ graspSet->addGrasp(preGrasp);
+
graspObject->addGraspSet(graspSet);
}
@@ -847,6 +858,7 @@ bool ReachabilityMapWindow::buildReachMapAll()
return false;
}
+
Eigen::Vector3f minBB, maxBB;
reachSpace->getWorkspaceExtends(minBB, maxBB);
reachGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), reachSpace->getDiscretizeParameterTranslation()));
@@ -874,6 +886,9 @@ bool ReachabilityMapWindow::buildReachMap(VirtualRobot::GraspPtr g)
return false;
}
+ return false;
+
+
reachSpace->getWorkspaceExtends(minBB, maxBB);
reachGrid.reset(new WorkspaceGrid(minBB(0), maxBB(0), minBB(1), maxBB(1), reachSpace->getDiscretizeParameterTranslation()));
--
GitLab
From 8cb03f4177c5e5c0aa771e28e406d4a681d15b2a Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 18 Oct 2021 16:23:24 +0200
Subject: [PATCH 09/14] default initial posture
---
.../examples/reachability/reachabilityScene.cpp | 11 +++++++++++
.../examples/reachability/reachabilityWindow.cpp | 10 ++++++++++
2 files changed, 21 insertions(+)
diff --git a/VirtualRobot/examples/reachability/reachabilityScene.cpp b/VirtualRobot/examples/reachability/reachabilityScene.cpp
index 3b68353a7..c73a7f963 100644
--- a/VirtualRobot/examples/reachability/reachabilityScene.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityScene.cpp
@@ -55,6 +55,17 @@ void endlessExtend(std::string robotFile, std::string reachFile, int steps, unsi
try
{
robot = RobotIO::loadRobot(robotFile);
+
+ const std::map<std::string, float> jointValues =
+ {
+ {"ArmL5_Elb1", 1.375},
+ {"ArmR5_Elb1", 1.375}
+ };
+
+ std::cout << "Setting joint values\n\n";
+
+ robot->setJointValues(jointValues);
+
}
catch (VirtualRobotException& e)
{
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index 169052551..210a697c7 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -438,6 +438,16 @@ void reachabilityWindow::loadRobot()
try
{
robot = RobotIO::loadRobot(robotFile);
+
+ const std::map<std::string, float> jointValues =
+ {
+ {"ArmL5_Elb1", 1.375},
+ {"ArmR5_Elb1", 1.375}
+ };
+
+ std::cout << "Setting joint values\n\n";
+
+ robot->setJointValues(jointValues);
}
catch (VirtualRobotException& e)
{
--
GitLab
From 642d78bfa44cd79fd747a04c65470afd0116ad19 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 18 Oct 2021 16:24:12 +0200
Subject: [PATCH 10/14] VirtualRobot/IK: setting names
---
VirtualRobot/IK/Constraint.cpp | 2 ++
VirtualRobot/IK/DifferentialIK.cpp | 2 ++
VirtualRobot/IK/HierarchicalIK.cpp | 7 ++++++-
VirtualRobot/IK/JacobiProvider.cpp | 2 +-
VirtualRobot/IK/JointLimitAvoidanceJacobi.cpp | 1 +
VirtualRobot/IK/constraints/BalanceConstraint.cpp | 4 +++-
VirtualRobot/IK/constraints/PoseConstraint.cpp | 2 ++
.../IK/constraints/ReferenceConfigurationConstraint.cpp | 3 +++
VirtualRobot/IK/constraints/TSRConstraint.cpp | 1 +
9 files changed, 21 insertions(+), 3 deletions(-)
diff --git a/VirtualRobot/IK/Constraint.cpp b/VirtualRobot/IK/Constraint.cpp
index c7ea22bf7..f793053f0 100644
--- a/VirtualRobot/IK/Constraint.cpp
+++ b/VirtualRobot/IK/Constraint.cpp
@@ -8,6 +8,8 @@ Constraint::Constraint(const RobotNodeSetPtr& nodeSet) :
lastLastError(-1),
optimizationFunctionFactor(1)
{
+ name ="Constraint";
+ initialized = true;
}
void Constraint::initialize()
diff --git a/VirtualRobot/IK/DifferentialIK.cpp b/VirtualRobot/IK/DifferentialIK.cpp
index 00e737f31..39d4ee310 100644
--- a/VirtualRobot/IK/DifferentialIK.cpp
+++ b/VirtualRobot/IK/DifferentialIK.cpp
@@ -44,6 +44,8 @@ namespace VirtualRobot
positionMaxStep = -1.0f;
tmpUpdateErrorDelta.resize(6);
+
+ name = "DifferentialIK";
}
diff --git a/VirtualRobot/IK/HierarchicalIK.cpp b/VirtualRobot/IK/HierarchicalIK.cpp
index 049cbdfa0..a45b37ba2 100644
--- a/VirtualRobot/IK/HierarchicalIK.cpp
+++ b/VirtualRobot/IK/HierarchicalIK.cpp
@@ -1,6 +1,7 @@
#include "HierarchicalIK.h"
#include <VirtualRobot/MathTools.h>
+#include "VirtualRobot.h"
using namespace VirtualRobot;
using namespace std;
@@ -36,7 +37,7 @@ namespace VirtualRobot
VR_INFO << "Compute Step" << std::endl;
}
- int ndof = jacDefs[0]->getRobotNodeSet()->getSize();
+ uint ndof = jacDefs.front()->getRobotNodeSet()->getSize();
Eigen::VectorXf result(ndof);
result.setZero();
std::vector<Eigen::MatrixXd> jacobies;
@@ -45,6 +46,8 @@ namespace VirtualRobot
for (size_t i = 0; i < jacDefs.size(); i++)
{
+ jacDefs[i]->print();
+
THROW_VR_EXCEPTION_IF(!jacDefs[i]->isInitialized(), "JacobiProvider is not initialized...");
Eigen::MatrixXd j = jacDefs[i]->getJacobianMatrixD();// jacDefs[i].tcp);
jacobies.push_back(j);
@@ -75,6 +78,8 @@ namespace VirtualRobot
}
}
+ VR_ASSERT(jacobies.size() == jacDefs.size());
+
// generate hierarchical gradient descent
// init with first jacobi
diff --git a/VirtualRobot/IK/JacobiProvider.cpp b/VirtualRobot/IK/JacobiProvider.cpp
index 86b0c92f8..aee5347f7 100644
--- a/VirtualRobot/IK/JacobiProvider.cpp
+++ b/VirtualRobot/IK/JacobiProvider.cpp
@@ -15,7 +15,7 @@ namespace VirtualRobot
using std::endl;
JacobiProvider::JacobiProvider(RobotNodeSetPtr rns, InverseJacobiMethod invJacMethod) :
- name("JacobiProvvider"), rns(rns), inverseMethod(invJacMethod)
+ name("JacobiProvider"), rns(rns), inverseMethod(invJacMethod)
{
initialized = false;
dampedSvdLambda = 0.1;
diff --git a/VirtualRobot/IK/JointLimitAvoidanceJacobi.cpp b/VirtualRobot/IK/JointLimitAvoidanceJacobi.cpp
index ea2cbbcbc..dca2dedca 100644
--- a/VirtualRobot/IK/JointLimitAvoidanceJacobi.cpp
+++ b/VirtualRobot/IK/JointLimitAvoidanceJacobi.cpp
@@ -16,6 +16,7 @@ namespace VirtualRobot
{
nodes = rns->getAllRobotNodes();
initialized = true; // no need of spiecial initialization
+ name = "JointLimitAvoidanceJacobi";
VR_ASSERT(nodes.size() > 0);
}
diff --git a/VirtualRobot/IK/constraints/BalanceConstraint.cpp b/VirtualRobot/IK/constraints/BalanceConstraint.cpp
index 6de29ce36..9743b9834 100644
--- a/VirtualRobot/IK/constraints/BalanceConstraint.cpp
+++ b/VirtualRobot/IK/constraints/BalanceConstraint.cpp
@@ -142,6 +142,7 @@ BalanceConstraint::BalanceConstraint(const RobotPtr& robot, const RobotNodeSetPt
Constraint(joints)
{
initialize(robot, joints, bodies, contactNodes, tolerance, minimumStability, maxSupportDistance, supportPolygonUpdates, considerCoMHeight);
+ name = "BalanceConstraint";
}
BalanceConstraint::BalanceConstraint(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const SupportPolygonPtr& supportPolygon,
@@ -149,6 +150,8 @@ BalanceConstraint::BalanceConstraint(const RobotPtr& robot, const RobotNodeSetPt
Constraint(joints)
{
initialize(robot, joints, bodies, supportPolygon->getContactModels(), tolerance, minimumStability, maxSupportDistance, supportPolygonUpdates, considerCoMHeight);
+ name = "BalanceConstraint";
+
}
void BalanceConstraint::initialize(const RobotPtr& robot, const RobotNodeSetPtr& joints, const RobotNodeSetPtr& bodies, const SceneObjectSetPtr& contactNodes,
@@ -321,4 +324,3 @@ SupportPolygonPtr BalanceConstraint::getSupportPolygon()
{
return supportPolygon;
}
-
diff --git a/VirtualRobot/IK/constraints/PoseConstraint.cpp b/VirtualRobot/IK/constraints/PoseConstraint.cpp
index c88cbf5a8..7df329e76 100644
--- a/VirtualRobot/IK/constraints/PoseConstraint.cpp
+++ b/VirtualRobot/IK/constraints/PoseConstraint.cpp
@@ -53,6 +53,8 @@ PoseConstraint::PoseConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nod
addOptimizationFunction(ORIENTATION_COMPONENT);
initialized = true;
+
+ name = "PoseConstraint";
}
void PoseConstraint::setVisualization(const SceneObjectSetPtr& visualizationNodeSet)
diff --git a/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.cpp b/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.cpp
index 293998f9b..1da475181 100644
--- a/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.cpp
+++ b/VirtualRobot/IK/constraints/ReferenceConfigurationConstraint.cpp
@@ -36,6 +36,7 @@ ReferenceConfigurationConstraint::ReferenceConfigurationConstraint(const RobotPt
// Reference configuration is considered a soft constraint
addOptimizationFunction(0, true);
+ name = "ReferenceConfigurationConstraint";
initialized = true;
}
@@ -50,6 +51,8 @@ ReferenceConfigurationConstraint::ReferenceConfigurationConstraint(const RobotPt
// Joint limit avoidance is considered a soft constraint
addOptimizationFunction(0, true);
+ name = "ReferenceConfigurationConstraint";
+
initialized = true;
}
diff --git a/VirtualRobot/IK/constraints/TSRConstraint.cpp b/VirtualRobot/IK/constraints/TSRConstraint.cpp
index 882a1d137..aaf016c5d 100644
--- a/VirtualRobot/IK/constraints/TSRConstraint.cpp
+++ b/VirtualRobot/IK/constraints/TSRConstraint.cpp
@@ -55,6 +55,7 @@ TSRConstraint::TSRConstraint(const RobotPtr& robot, const RobotNodeSetPtr& nodeS
addOptimizationFunction(POSITION_COMPONENT);
addOptimizationFunction(ORIENTATION_COMPONENT);
+ name = "TSRConstraint";
initialized = true;
}
--
GitLab
From 26cb96b67469f25fa36461f0b79a4eac3b53cc60 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 18 Oct 2021 16:24:46 +0200
Subject: [PATCH 11/14] RobotNode: struct for JointLimts
---
VirtualRobot/Nodes/RobotNode.h | 16 +++++++++++++++-
1 file changed, 15 insertions(+), 1 deletion(-)
diff --git a/VirtualRobot/Nodes/RobotNode.h b/VirtualRobot/Nodes/RobotNode.h
index 0c3a40f32..89ddb7531 100644
--- a/VirtualRobot/Nodes/RobotNode.h
+++ b/VirtualRobot/Nodes/RobotNode.h
@@ -206,6 +206,21 @@ namespace VirtualRobot
float getJointLimitLo();
float getJointLimitHi();
+ struct JointLimits
+ {
+ float low;
+ float high;
+ };
+
+ JointLimits getJointLimits()
+ {
+ return JointLimits
+ {
+ .low = getJointLimitLo(),
+ .high = getJointLimitHi()
+ };
+ }
+
/*!
Set joint limits [rad].
*/
@@ -463,4 +478,3 @@ namespace VirtualRobot
};
} // namespace VirtualRobot
-
--
GitLab
From 04ab34dc944806758b23528db2ad24c5ae59f369 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Mon, 20 Dec 2021 21:26:46 +0100
Subject: [PATCH 12/14] snapshot
---
SimoxUtility/math/convert/pos_rpy_to_mat4f.h | 10 ++---
VirtualRobot/Workspace/WorkspaceGrid.cpp | 26 ++++++++++---
VirtualRobot/Workspace/WorkspaceGrid.h | 37 ++++++++++++++++++-
.../Workspace/WorkspaceRepresentation.cpp | 4 +-
.../Workspace/WorkspaceRepresentation.h | 5 +--
VirtualRobot/XML/RobotIO.h | 1 -
.../ReachabilityMapWindow.cpp | 3 --
.../ReachabilityMap/ReachabilityMapWindow.cpp | 9 ++++-
8 files changed, 73 insertions(+), 22 deletions(-)
diff --git a/SimoxUtility/math/convert/pos_rpy_to_mat4f.h b/SimoxUtility/math/convert/pos_rpy_to_mat4f.h
index f8bbd3cce..a0ef50524 100644
--- a/SimoxUtility/math/convert/pos_rpy_to_mat4f.h
+++ b/SimoxUtility/math/convert/pos_rpy_to_mat4f.h
@@ -5,7 +5,7 @@
#include "rpy_to_mat4f.h"
//out param version
-namespace simox::math
+namespace simox::math::detail
{
template<class D1> inline
@@ -51,7 +51,7 @@ namespace simox::math
inline Eigen::Matrix4f pos_rpy_to_mat4f(float x, float y, float z, float roll, float pitch, float yaw)
{
Eigen::Matrix4f m4;
- pos_rpy_to_mat4f(x, y, z, roll, pitch, yaw, m4);
+ detail::pos_rpy_to_mat4f(x, y, z, roll, pitch, yaw, m4);
return m4;
}
@@ -60,7 +60,7 @@ namespace simox::math
pos_rpy_to_mat4f(const Eigen::MatrixBase<D1>& pos, float roll, float pitch, float yaw)
{
Eigen::Matrix4f m4;
- pos_rpy_to_mat4f(pos, roll, pitch, yaw, m4);
+ detail::pos_rpy_to_mat4f(pos, roll, pitch, yaw, m4);
return m4;
}
@@ -69,7 +69,7 @@ namespace simox::math
pos_rpy_to_mat4f(float x, float y, float z, const Eigen::MatrixBase<D1>& rpy)
{
Eigen::Matrix4f m4;
- pos_rpy_to_mat4f(x, y, z, rpy, m4);
+ detail::pos_rpy_to_mat4f(x, y, z, rpy, m4);
return m4;
}
@@ -78,7 +78,7 @@ namespace simox::math
pos_rpy_to_mat4f(const Eigen::MatrixBase<D1>& pos, const Eigen::MatrixBase<D2>& rpy)
{
Eigen::Matrix4f m4;
- pos_rpy_to_mat4f(pos, rpy, m4);
+ detail::pos_rpy_to_mat4f(pos, rpy, m4);
return m4;
}
}
diff --git a/VirtualRobot/Workspace/WorkspaceGrid.cpp b/VirtualRobot/Workspace/WorkspaceGrid.cpp
index 865f907ea..df59aa675 100644
--- a/VirtualRobot/Workspace/WorkspaceGrid.cpp
+++ b/VirtualRobot/Workspace/WorkspaceGrid.cpp
@@ -4,8 +4,12 @@
#include "../ManipulationObject.h"
#include "../Nodes/RobotNode.h"
#include "../Robot.h"
+#include "Workspace/WorkspaceData.h"
+#include <cmath>
#include <iostream>
#include <algorithm>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Transform.h>
using namespace std;
#define MIN_VALUES_STORE_GRASPS 100
@@ -235,6 +239,15 @@ namespace VirtualRobot
}
}
+ Eigen::Vector2f WorkspaceGrid::getPosition(int cellX, int cellY) const
+ {
+ float x = minX + cellX / gridSizeX * gridExtendX;
+ float y = minY + cellY / gridSizeY * gridExtendY;
+
+ return {x,y};
+
+ }
+
void WorkspaceGrid::setEntryCheckNeighbors(float x, float y, int value, GraspPtr grasp)
{
if (!data)
@@ -281,6 +294,7 @@ namespace VirtualRobot
x = tmpPos2(0, 3);
y = tmpPos2(1, 3);
+
// setCellEntry(x, y, i->value, grasp);
@@ -437,7 +451,7 @@ namespace VirtualRobot
maxY = y + gridExtendY / 2.0f;
}
- bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode)
+ bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode, const float baseOrientation)
{
if (!ws || !o || !g)
{
@@ -446,10 +460,10 @@ namespace VirtualRobot
Eigen::Matrix4f graspGlobal = g->getTcpPoseGlobal(o->getGlobalPose());
- return fillGridData(ws, graspGlobal, g, baseRobotNode);
+ return fillGridData(ws, graspGlobal, g, baseRobotNode, baseOrientation);
}
- bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &graspGlobal, GraspPtr g, RobotNodePtr baseRobotNode)
+ bool WorkspaceGrid::fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &graspGlobal, GraspPtr g, RobotNodePtr baseRobotNode, const float baseOrientation)
{
if (!ws)
{
@@ -461,7 +475,10 @@ namespace VirtualRobot
if (baseRobotNode)
{
gpOrig = baseRobotNode->getRobot()->getGlobalPose();
- baseRobotNode->getRobot()->setGlobalPose(Eigen::Matrix4f::Identity());
+
+ Eigen::Isometry3f baseRobotNodePose = Eigen::Isometry3f::Identity();
+ baseRobotNodePose.linear() = Eigen::AngleAxisf(baseOrientation, Eigen::Vector3f::UnitZ()).toRotationMatrix();
+ baseRobotNode->getRobot()->setGlobalPose(baseRobotNodePose.matrix());
}
WorkspaceRepresentation::WorkspaceCut2DPtr cutXY = ws->createCut(graspGlobal, discretizeSize, false);
@@ -550,4 +567,3 @@ namespace VirtualRobot
}
} // namespace
-
diff --git a/VirtualRobot/Workspace/WorkspaceGrid.h b/VirtualRobot/Workspace/WorkspaceGrid.h
index 40bdef80f..0b97744d5 100644
--- a/VirtualRobot/Workspace/WorkspaceGrid.h
+++ b/VirtualRobot/Workspace/WorkspaceGrid.h
@@ -90,8 +90,8 @@ namespace VirtualRobot
/*!
Fill the grid with inverse reachability data generated from grasp g and object o.
*/
- bool fillGridData(WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode);
- bool fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &graspGlobal, GraspPtr g, RobotNodePtr baseRobotNode);
+ bool fillGridData(WorkspaceRepresentationPtr ws, ManipulationObjectPtr o, GraspPtr g, RobotNodePtr baseRobotNode, float baseOrientation = 0);
+ bool fillGridData(WorkspaceRepresentationPtr ws, const Eigen::Matrix4f &graspGlobal, GraspPtr g, RobotNodePtr baseRobotNode, float baseOrientation = 0);
/*!
@@ -104,12 +104,45 @@ namespace VirtualRobot
*/
void getExtends(float& storeMinX, float& storeMaxX, float& storeMinY, float& storeMaxY);
+ struct Extends
+ {
+ float minX;
+ float maxX;
+
+ float minY;
+ float maxY;
+ };
+
+ Extends getExtends() const
+ {
+ return Extends
+ {
+ .minX = minX,
+ .maxX = maxX,
+ .minY = minY,
+ .maxY = maxY
+ };
+ }
+
/*!
Number of cells in x and y
*/
void getCells(int& storeCellsX, int& storeCellsY);
+ struct Size
+ {
+ int x,y;
+ };
+
+ Size getCells() const
+ {
+ return Size{.x = gridSizeX, .y = gridSizeY};
+ }
+
+
+ Eigen::Vector2f getPosition(int cellX, int cellY) const;
+
float getDiscretizeSize() const;
Eigen::Vector2f getMin() const;
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
index 750a1ac4b..382a38add 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.cpp
@@ -1163,8 +1163,8 @@ namespace VirtualRobot
void WorkspaceRepresentation::initialize(RobotNodeSetPtr nodeSet,
float discretizeStepTranslation,
float discretizeStepRotation,
- float minBounds[6],
- float maxBounds[6],
+ const float minBounds[6],
+ const float maxBounds[6],
SceneObjectSetPtr staticCollisionModel,
SceneObjectSetPtr dynamicCollisionModel,
RobotNodePtr baseNode /*= RobotNodePtr()*/,
diff --git a/VirtualRobot/Workspace/WorkspaceRepresentation.h b/VirtualRobot/Workspace/WorkspaceRepresentation.h
index 38ea7722a..d90978494 100644
--- a/VirtualRobot/Workspace/WorkspaceRepresentation.h
+++ b/VirtualRobot/Workspace/WorkspaceRepresentation.h
@@ -134,8 +134,8 @@ namespace VirtualRobot
virtual void initialize(RobotNodeSetPtr nodeSet,
float discretizeStepTranslation,
float discretizeStepRotation,
- float minBounds[6],
- float maxBounds[6],
+ const float minBounds[6],
+ const float maxBounds[6],
SceneObjectSetPtr staticCollisionModel = SceneObjectSetPtr(),
SceneObjectSetPtr dynamicCollisionModel = SceneObjectSetPtr(),
RobotNodePtr baseNode = RobotNodePtr(),
@@ -495,4 +495,3 @@ namespace VirtualRobot
};
} // namespace VirtualRobot
-
diff --git a/VirtualRobot/XML/RobotIO.h b/VirtualRobot/XML/RobotIO.h
index b1e9d11a1..0f5fcec20 100644
--- a/VirtualRobot/XML/RobotIO.h
+++ b/VirtualRobot/XML/RobotIO.h
@@ -144,4 +144,3 @@ namespace VirtualRobot
};
}
-
diff --git a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
index 5e08820bc..aa62199f3 100644
--- a/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
+++ b/VirtualRobot/examples/PlatformPlacement/ReachabilityMapWindow.cpp
@@ -10,9 +10,6 @@
#include <VirtualRobot/Workspace/WorkspaceGrid.h>
#include <QFileDialog>
#include <Eigen/Geometry>
-#include <Eigen/src/Geometry/AngleAxis.h>
-#include <Eigen/src/Geometry/Transform.h>
-#include <Eigen/src/Geometry/Translation.h>
#include <cstdlib>
#include <ctime>
#include <vector>
diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
index 99265fcd8..a52bf5b90 100644
--- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
+++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
@@ -16,6 +16,7 @@
#include <cmath>
#include "Inventor/actions/SoLineHighlightRenderAction.h"
+#include "SimoxUtility/math/convert/mat4f_to_rpy.h"
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
@@ -443,7 +444,7 @@ void ReachabilityMapWindow::buildReachGridVisu()
reachabilityMapVisuSep->removeAllChildren();
- SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachGrid, VirtualRobot::ColorMap::eHot, true);
+ SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachGrid, VirtualRobot::ColorMap::eGray, true);
if (visualisationNode)
{
@@ -548,6 +549,8 @@ void ReachabilityMapWindow::selectEEF(int nr)
eef = eefs[nr];
+
+
if (graspObject)
{
grasps = graspObject->getGraspSet(eef);
@@ -775,6 +778,10 @@ bool ReachabilityMapWindow::buildReachMap(VirtualRobot::GraspPtr g)
Eigen::Matrix4f gp = graspObject->getGlobalPose();
reachGrid->setGridPosition(gp(0, 3), gp(1, 3));
+ VR_INFO << "Filling grid for grasp object " << gp;
+ VR_INFO << "Filling grid for grasp " << g->getTcpPoseGlobal(gp);
+ VR_INFO << "Ori" << simox::math::mat4f_to_rpy(g->getTcpPoseGlobal(gp));
+
reachGrid->fillGridData(reachSpace, graspObject, g, robot->getRootNode());
updateVisu();
--
GitLab
From 01eba2b4f614a3c810532efc003484dfe9ee877f Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 Jan 2022 09:17:48 +0100
Subject: [PATCH 13/14] swapping args of simox::alg::apply
---
SimoxUtility/algorithm/apply.hpp | 7 ++++---
SimoxUtility/color/ColorMap.h | 4 ++--
2 files changed, 6 insertions(+), 5 deletions(-)
diff --git a/SimoxUtility/algorithm/apply.hpp b/SimoxUtility/algorithm/apply.hpp
index 85029e5b9..b28f06ac1 100644
--- a/SimoxUtility/algorithm/apply.hpp
+++ b/SimoxUtility/algorithm/apply.hpp
@@ -11,10 +11,12 @@ namespace simox::alg
template <typename ValueIn, typename UnaryOp>
std::vector<std::invoke_result_t<UnaryOp, ValueIn>>
- apply(const UnaryOp& op, const std::vector<ValueIn>& vector)
+ apply(const std::vector<ValueIn>& vector, const UnaryOp& op)
{
using ValueOut = std::invoke_result_t<UnaryOp, ValueIn>;
std::vector<ValueOut> result;
+ result.reserve(vector.size());
+
std::transform(vector.begin(), vector.end(), std::back_inserter(result), op);
return result;
}
@@ -22,7 +24,7 @@ namespace simox::alg
template <typename Key, typename ValueIn, typename UnaryOp>
std::map<Key, std::invoke_result_t<UnaryOp, ValueIn>>
- apply(const UnaryOp& op, const std::map<Key, ValueIn>& map)
+ apply(const std::map<Key, ValueIn>& map, const UnaryOp& op)
{
using ValueOut = std::invoke_result_t<UnaryOp, ValueIn>;
std::map<Key, ValueOut> result;
@@ -35,4 +37,3 @@ namespace simox::alg
}
-
diff --git a/SimoxUtility/color/ColorMap.h b/SimoxUtility/color/ColorMap.h
index 951b83cae..06b6bdea3 100644
--- a/SimoxUtility/color/ColorMap.h
+++ b/SimoxUtility/color/ColorMap.h
@@ -91,13 +91,13 @@ namespace simox::color
template <typename V>
std::vector<Color> operator()(const std::vector<V>& vector) const
{
- return simox::alg::apply(*this, vector);
+ return simox::alg::apply(vector, *this);
}
/// Apply this colormap to a map's values.
template <typename K, typename V>
std::map<K, Color> operator()(const std::map<K, V>& map) const
{
- return simox::alg::apply(*this, map);
+ return simox::alg::apply(map, *this);
}
--
GitLab
From 6c45c40618662a71f76dcaff791131c39bd68024 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Wed, 12 Jan 2022 09:18:10 +0100
Subject: [PATCH 14/14] snapshot
---
VirtualRobot/Workspace/WorkspaceGrid.cpp | 3 ++
.../ReachabilityMap/ReachabilityMapWindow.cpp | 42 ++++++++++++++++++-
.../reachability/reachabilityWindow.cpp | 4 +-
3 files changed, 46 insertions(+), 3 deletions(-)
diff --git a/VirtualRobot/Workspace/WorkspaceGrid.cpp b/VirtualRobot/Workspace/WorkspaceGrid.cpp
index df59aa675..deab899ca 100644
--- a/VirtualRobot/Workspace/WorkspaceGrid.cpp
+++ b/VirtualRobot/Workspace/WorkspaceGrid.cpp
@@ -4,6 +4,7 @@
#include "../ManipulationObject.h"
#include "../Nodes/RobotNode.h"
#include "../Robot.h"
+#include "VirtualRobot.h"
#include "Workspace/WorkspaceData.h"
#include <cmath>
#include <iostream>
@@ -472,6 +473,8 @@ namespace VirtualRobot
// ensure robot is at identity
Eigen::Matrix4f gpOrig = Eigen::Matrix4f::Identity();
+
+ VR_ASSERT(baseRobotNode);
if (baseRobotNode)
{
gpOrig = baseRobotNode->getRobot()->getGlobalPose();
diff --git a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
index a52bf5b90..d4e9a3cdf 100644
--- a/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
+++ b/VirtualRobot/examples/ReachabilityMap/ReachabilityMapWindow.cpp
@@ -10,6 +10,9 @@
#include <VirtualRobot/Workspace/WorkspaceGrid.h>
#include <QFileDialog>
#include <Eigen/Geometry>
+#include <Eigen/src/Core/Matrix.h>
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <Eigen/src/Geometry/Transform.h>
#include <ctime>
#include <vector>
#include <iostream>
@@ -17,6 +20,8 @@
#include "Inventor/actions/SoLineHighlightRenderAction.h"
#include "SimoxUtility/math/convert/mat4f_to_rpy.h"
+#include "VirtualRobot/Grasping/GraspSet.h"
+#include "VirtualRobot/VirtualRobot.h"
#include <Inventor/nodes/SoShapeHints.h>
#include <Inventor/nodes/SoLightModel.h>
@@ -284,6 +289,9 @@ void ReachabilityMapWindow::buildReachVisu()
{
if (!robot || !reachSpace || !graspObject || !eef)
{
+ VR_ASSERT(false);
+ VR_ERROR;
+
return;
}
@@ -294,6 +302,7 @@ void ReachabilityMapWindow::buildReachVisu()
if (!gs || gs->getSize() == 0)
{
+ VR_ERROR;
return;
}
@@ -317,11 +326,14 @@ void ReachabilityMapWindow::buildReachVisu()
SoNode *visualisationNode = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), Eigen::Vector3f::UnitZ(), maxCoeff);
+ VR_ASSERT(visualisationNode);
+
if (visualisationNode)
{
if (reachSpace->getBaseNode())
{
Eigen::Matrix4f gp = reachSpace->getBaseNode()->getGlobalPose();
+ VR_INFO << "reach space base node "<< gp;
reachabilityVisuSep->addChild(CoinVisualizationFactory::getMatrixTransform(gp));
}
reachabilityVisuSep->addChild(visualisationNode);
@@ -444,7 +456,9 @@ void ReachabilityMapWindow::buildReachGridVisu()
reachabilityMapVisuSep->removeAllChildren();
- SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachGrid, VirtualRobot::ColorMap::eGray, true);
+ SoNode* visualisationNode = CoinVisualizationFactory::getCoinVisualization(reachGrid, VirtualRobot::ColorMap::eHot, true);
+
+ VR_ASSERT(visualisationNode);
if (visualisationNode)
{
@@ -734,6 +748,32 @@ void ReachabilityMapWindow::loadObjectFile(std::string filename)
try
{
graspObject = ObjectIO::loadManipulationObject(filename);
+
+ Eigen::Isometry3f pose1 = Eigen::Isometry3f::Identity();
+ pose1.linear() = Eigen::AngleAxisf(M_PIf32, Eigen::Vector3f::UnitY()).toRotationMatrix() *
+ Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitZ()).toRotationMatrix()* Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX()).toRotationMatrix();
+
+ Eigen::Isometry3f pose2 = Eigen::Isometry3f::Identity();
+ pose2.linear() = Eigen::AngleAxisf(M_PIf32, Eigen::Vector3f::UnitY()).toRotationMatrix()
+ * Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX()).toRotationMatrix();
+
+
+ Eigen::Isometry3f pose3 = Eigen::Isometry3f::Identity();
+ pose3.linear() = Eigen::AngleAxisf(M_PIf32, Eigen::Vector3f::UnitY()).toRotationMatrix() *
+ Eigen::AngleAxisf(-M_PI_2f32, Eigen::Vector3f::UnitZ()).toRotationMatrix()* Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX()).toRotationMatrix();
+
+ Eigen::Isometry3f pose4 = Eigen::Isometry3f::Identity();
+ pose4.linear() = Eigen::AngleAxisf(M_PIf32, Eigen::Vector3f::UnitY()).toRotationMatrix() *
+ Eigen::AngleAxisf(-M_PIf32, Eigen::Vector3f::UnitZ()).toRotationMatrix()* Eigen::AngleAxisf(M_PI_2f32, Eigen::Vector3f::UnitX()).toRotationMatrix();
+
+
+ auto grasp1 = std::make_shared<Grasp>("1","Armar6","Hand_R_EEF", pose1.matrix());
+ auto grasp2 = std::make_shared<Grasp>("2","Armar6","Hand_R_EEF", pose2.matrix());
+ auto grasp3 = std::make_shared<Grasp>("3","Armar6","Hand_R_EEF", pose3.matrix());
+ auto grasp4 = std::make_shared<Grasp>("4","Armar6","Hand_R_EEF", pose4.matrix());
+
+ auto graspSet = std::make_shared<GraspSet>("Armar6", "Armar6", "Hand_R_EEF", std::vector<GraspPtr>{grasp1, grasp2, grasp3, grasp4});
+ graspObject->addGraspSet(graspSet);
}
catch(const VirtualRobotException &e)
{
diff --git a/VirtualRobot/examples/reachability/reachabilityWindow.cpp b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
index 210a697c7..35c42f1b0 100644
--- a/VirtualRobot/examples/reachability/reachabilityWindow.cpp
+++ b/VirtualRobot/examples/reachability/reachabilityWindow.cpp
@@ -222,7 +222,7 @@ void reachabilityWindow::reachVisu()
WorkspaceRepresentation::WorkspaceCut2DPtr cutData = reachSpace->createCut(pos,reachSpace->getDiscretizeParameterTranslation(), true);
VR_INFO << "Slider pos: " << pos << ", maxEntry:" << reachSpace->getMaxSummedAngleReachablity() << ", cut maxCoeff:" << cutData->entries.maxCoeff() << std::endl;
- SoNode *reachvisu2 = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGray), Eigen::Vector3f::UnitZ(), reachSpace->getMaxSummedAngleReachablity(), minAngle, maxAngle);
+ SoNode *reachvisu2 = CoinVisualizationFactory::getCoinVisualization(cutData, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), Eigen::Vector3f::UnitZ(), reachSpace->getMaxSummedAngleReachablity(), minAngle, maxAngle);
visualisationNode->addChild(reachvisu2);
}
@@ -235,7 +235,7 @@ void reachabilityWindow::reachVisu()
reachSpace->getWorkspaceExtends(minBB, maxBB);
float zDist = maxBB(2) - minBB(2);
float maxZ = minBB(2) + heightPercent*zDist - reachSpace->getDiscretizeParameterTranslation();
- SoNode *reachvisu = CoinVisualizationFactory::getCoinVisualization(reachSpace, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eGray), true, maxZ, minAngle, maxAngle);
+ SoNode *reachvisu = CoinVisualizationFactory::getCoinVisualization(reachSpace, VirtualRobot::ColorMap(VirtualRobot::ColorMap::eHot), true, maxZ, minAngle, maxAngle);
visualisationNode->addChild(reachvisu);
} else
--
GitLab