diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
index e822ef4fccf474aad6867531ba9ce7387724f17c..1d7b1191814046a2a6e5aa0d43a66d96b4203f1c 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.cpp
@@ -196,7 +196,8 @@ namespace GraspStudio
GraspEvaluationPoseUncertainty::PoseEvalResult GraspEvaluationPoseUncertainty::evaluatePose(
EndEffectorPtr eef, ObstaclePtr object, const Matrix4f& objectPose,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape,
+ float closingStepSize, float stepSizeSpeedFactor)
{
PoseEvalResult result;
result.forceClosure = false;
@@ -217,7 +218,7 @@ namespace GraspStudio
}
else
{
- eef->openActors();
+ eef->openActors(nullptr, closingStepSize, stepSizeSpeedFactor);
}
object->setGlobalPose(objectPose);
@@ -229,7 +230,7 @@ namespace GraspStudio
}
// collision free
- EndEffector::ContactInfoVector cont = eef->closeActors(object);
+ EndEffector::ContactInfoVector cont = eef->closeActors(object, closingStepSize, stepSizeSpeedFactor);
qm->setContactPoints(cont);
result.quality = qm->getGraspQuality();
@@ -240,28 +241,24 @@ namespace GraspStudio
GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluatePoses(
EndEffectorPtr eef, ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape)
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape,
+ float closingStepSize, float stepSizeSpeedFactor)
{
- PoseEvalResults res;
- res.avgQuality = 0.0f;
- res.forceClosureRate = 0.0f;
- res.avgQualityCol = 0.0f;
- res.forceClosureRateCol = 0.0f;
- res.numPosesTested = 0;
- res.numValidPoses = 0;
- res.numColPoses = 0;
-
+ if (objectPoses.empty())
+ {
+ return {};
+ }
if (!eef || !qm)
{
VR_ERROR << "Missing parameters" << endl;
- return res;
+ return {};
}
if (!eef->getRobot())
{
VR_WARNING << "missing eef->robot" << endl;
- return res;
+ return {};
}
Eigen::Matrix4f eefRobotPoseInit = eef->getRobot()->getGlobalPose();
@@ -269,16 +266,13 @@ namespace GraspStudio
VirtualRobot::RobotConfigPtr initialConf = eef->getConfiguration();
std::vector<PoseEvalResult> results;
+ results.reserve(objectPoses.size());
for (const auto& objectPose : objectPoses)
{
- results.push_back(evaluatePose(eef, object, objectPose, qm, preshape));
- }
-
- if (results.empty())
- {
- return res;
+ results.emplace_back(evaluatePose(eef, object, objectPose, qm, preshape, closingStepSize, stepSizeSpeedFactor));
}
+ PoseEvalResults res;
res.numPosesTested = static_cast<int>(results.size());
for (const auto& result : results)
{
@@ -321,7 +315,8 @@ namespace GraspStudio
GraspEvaluationPoseUncertainty::PoseEvalResults GraspEvaluationPoseUncertainty::evaluateGrasp(
VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
- GraspQualityMeasurePtr qm, int numPoses)
+ GraspQualityMeasurePtr qm, int numPoses,
+ float closingStepSize, float stepSizeSpeedFactor)
{
PoseEvalResults res;
res.avgQuality = 0.0f;
@@ -363,12 +358,12 @@ namespace GraspStudio
}
else
{
- eef->openActors();
+ eef->openActors(nullptr, closingStepSize, stepSizeSpeedFactor);
}
- auto contacts = eef->closeActors(object);
+ auto contacts = eef->closeActors(object, closingStepSize, stepSizeSpeedFactor);
if (contacts.empty())
{
VR_INFO << "No contacts for grasp " << grasp->getName() << " found" << std::endl;
@@ -381,7 +376,7 @@ namespace GraspStudio
VR_INFO << "No poses for grasp found" << std::endl;
return res;
}
- res = evaluatePoses(eef, object, poses, qm, graspPS);
+ res = evaluatePoses(eef, object, poses, qm, graspPS, closingStepSize, stepSizeSpeedFactor);
// restore setup
eef->getRobot()->setGlobalPose(eefRobotPoseInit);
diff --git a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
index 6ae74d6867317c9b32ca3fcc15de0e52c9b07172..649fb95bccff9d0b02a71321eea5701e3c637d55 100644
--- a/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
+++ b/GraspPlanning/GraspQuality/GraspEvaluationPoseUncertainty.h
@@ -156,15 +156,16 @@ namespace GraspStudio
PoseEvalResult evaluatePose(
VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const Eigen::Matrix4f& objectPose,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {});
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {}, float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
PoseEvalResults evaluatePoses(
VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object, const std::vector<Eigen::Matrix4f>& objectPoses,
- GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {});
+ GraspQualityMeasurePtr qm, VirtualRobot::RobotConfigPtr preshape = {}, float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
PoseEvalResults evaluateGrasp(
VirtualRobot::GraspPtr grasp, VirtualRobot::EndEffectorPtr eef, VirtualRobot::ObstaclePtr object,
- GraspQualityMeasurePtr qm, int numPoses);
+ GraspQualityMeasurePtr qm, int numPoses,
+ float closingStepSize = 0.02f, float stepSizeSpeedFactor = 1);
protected:
diff --git a/VirtualRobot/EndEffector/EndEffector.cpp b/VirtualRobot/EndEffector/EndEffector.cpp
index ae403354f8eb38a9ede5a93be13e47b7b38f815c..6eecb5dbc5d630a3bf548ae4872943a7bca8787d 100644
--- a/VirtualRobot/EndEffector/EndEffector.cpp
+++ b/VirtualRobot/EndEffector/EndEffector.cpp
@@ -105,10 +105,21 @@ namespace VirtualRobot
{
actors = this->actors;
}
+
+
+ const std::vector<EndEffectorActorPtr>& EndEffector::getActors()
+ {
+ return actors;
+ }
+
void EndEffector::getStatics(std::vector<RobotNodePtr>& statics)
{
statics = this->statics;
}
+ const std::vector<RobotNodePtr>& EndEffector::getStatics()
+ {
+ return statics;
+ }
EndEffector::ContactInfoVector EndEffector::closeActors(SceneObjectSetPtr obstacles, float stepSize, float stepSizeSpeedFactor, std::uint64_t steps)
{
diff --git a/VirtualRobot/EndEffector/EndEffector.h b/VirtualRobot/EndEffector/EndEffector.h
index b45a1d1e8f6125a5e83edf3e04269b2e85cb53c3..63d36f08daf279b7748aa465f498009e0bda23c6 100644
--- a/VirtualRobot/EndEffector/EndEffector.h
+++ b/VirtualRobot/EndEffector/EndEffector.h
@@ -100,7 +100,10 @@ namespace VirtualRobot
std::string getRobotType();
void getActors(std::vector<EndEffectorActorPtr>& actors);
+ const std::vector<EndEffectorActorPtr>& getActors();
+
void getStatics(std::vector<RobotNodePtr>& statics);
+ const std::vector<RobotNodePtr>& getStatics();
/*!
Closes each actor until a joint limit is hit or a collision occurred.
diff --git a/VirtualRobot/EndEffector/EndEffectorActor.cpp b/VirtualRobot/EndEffector/EndEffectorActor.cpp
index 834a7a57226774ccdd73f3995e46e96654a43e9e..3637834f4cdc34ae1a199fe43942a425553e3332 100644
--- a/VirtualRobot/EndEffector/EndEffectorActor.cpp
+++ b/VirtualRobot/EndEffector/EndEffectorActor.cpp
@@ -92,31 +92,19 @@ namespace VirtualRobot
bool EndEffectorActor::moveActorCheckCollision(EndEffectorPtr eef, EndEffector::ContactInfoVector& storeContacts, SceneObjectSetPtr obstacles /*= SceneObjectSetPtr()*/, float angle /*= 0.02*/)
{
VR_ASSERT(eef);
- RobotPtr robot = eef->getRobot();
+ const RobotPtr& robot = eef->getRobot();
VR_ASSERT(robot);
//bool res = true;
- std::vector<EndEffectorActorPtr> eefActors;
- eef->getActors(eefActors);
- std::vector<RobotNodePtr> eefStatic;
- eef->getStatics(eefStatic);
EndEffector::ContactInfoVector newContacts;
- enum ActorStatus
- {
- eFinished,
- eCollision,
- eMoving
- };
- std::map<RobotNodePtr, ActorStatus> actorStatus;
-
-
+ bool allActorsHaveStopped = true;
for (auto& actor : actors)
{
float oldV = actor.robotNode->getJointValue();
float v = oldV + angle * actor.directionAndSpeed;
- actorStatus[actor.robotNode] = eMoving;
+ bool isMoving = true;
if (v <= actor.robotNode->getJointLimitHi() && v >= actor.robotNode->getJointLimitLo())
{
@@ -135,7 +123,7 @@ namespace VirtualRobot
// actors (don't store contacts)
if (!collision)
{
- for (auto& eefActor : eefActors)
+ for (auto& eefActor : eef->getActors())
{
// Don't check for collisions with the actor itself (don't store contacts)
if ((eefActor->getName() != name) && isColliding(eefActor)) //isColliding(eef,*a,newContacts) )
@@ -148,7 +136,7 @@ namespace VirtualRobot
// static (don't store contacts)
if (!collision)
{
- for (auto& node : eefStatic)
+ for (auto& node : eef->getStatics())
{
SceneObjectPtr so = boost::dynamic_pointer_cast<SceneObject>(node);
@@ -161,22 +149,22 @@ namespace VirtualRobot
}
}
- if (!collision)
- {
- //res = false;
- }
- else
+ if (collision)
{
// reset last position
//n->robotNode->setJointValue(oldV);
robot->setJointValue(actor.robotNode, oldV);
- actorStatus[actor.robotNode] = eCollision;
+ isMoving = false;
}
}
else
{
- actorStatus[actor.robotNode] = eFinished;
+ isMoving = false;
+ }
+ if (isMoving)
+ {
+ allActorsHaveStopped = false;
}
}
@@ -217,18 +205,7 @@ namespace VirtualRobot
}
}
- // check what we should return
- bool res = true;
- for (auto& actor : actors)
- {
- // if at least one actor is not in collision and not at its joint limits, we are still in the process of closing the fingers
- if (actorStatus[actor.robotNode] == eMoving)
- {
- res = false;
- }
- }
-
- return res;
+ return allActorsHaveStopped;
}
diff --git a/VirtualRobot/Robot.cpp b/VirtualRobot/Robot.cpp
index 09b435c34332ad5a2a48aaa5fb39eff5a0f124bc..3a909da3e70d9a03fe7587dc4b85ad398aad13d9 100644
--- a/VirtualRobot/Robot.cpp
+++ b/VirtualRobot/Robot.cpp
@@ -11,6 +11,7 @@
namespace VirtualRobot
{
+ const RobotPtr Robot::NullPtr{nullptr};
Robot::Robot(const std::string& name, const std::string& type)
: SceneObject(name)
, scaling(1.0f)
diff --git a/VirtualRobot/Robot.h b/VirtualRobot/Robot.h
index 274ce8ecb30d9fe02199ab2e7078745d6b841543..37f212272b2cb16f46dadc5fcc68645ddbd29bf0 100644
--- a/VirtualRobot/Robot.h
+++ b/VirtualRobot/Robot.h
@@ -51,6 +51,8 @@ namespace VirtualRobot
{
friend class RobotIO;
public:
+ static const RobotPtr NullPtr;
+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/*!
@@ -205,7 +207,7 @@ namespace VirtualRobot
virtual Eigen::Matrix4f getGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode) const;
//! Set the global pose of this robot so that the RobotNode node is at pose globalPoseNode.
virtual void setGlobalPoseForRobotNode(const RobotNodePtr& node, const Eigen::Matrix4f& globalPoseNode);
-
+
//! Get the global position of this robot so that the RobotNode node is at position globalPoseNode
virtual Eigen::Matrix4f getGlobalPositionForRobotNode(const RobotNodePtr& node, const Eigen::Vector3f& globalPositionNode) const;
//! Set the global position of this robot so that the RobotNode node is at position globalPoseNode
@@ -213,7 +215,7 @@ namespace VirtualRobot
//virtual Eigen::Matrix4f getGlobalPose() = 0;
-
+
//! Return center of mass of this robot in local coordinate frame. All RobotNodes of this robot are considered according to their mass.
Eigen::Vector3f getCoMLocal() override;
@@ -223,7 +225,7 @@ namespace VirtualRobot
//! Return accumulated mass of this robot.
virtual float getMass();
-
+
/*!
Extract a sub kinematic from this robot and create a new robot instance.
\param startJoint The kinematic starts with this RobotNode