Skip to content

Commit

Permalink
Updated version to the latest Golem and Grasp
Browse files Browse the repository at this point in the history
  • Loading branch information
@memnone
memnone committed Nov 19, 2015
1 parent b164cc0 commit 8598cdc
Show file tree
Hide file tree
Showing 16 changed files with 371 additions and 322 deletions.
6 changes: 4 additions & 2 deletions CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -270,7 +270,8 @@ if (WIN32)
TARGET_LINK_LIBRARIES(
SpamSpam
optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX}
optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX}
optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX}
#optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX}
optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX}
optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX}
optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX}
Expand Down Expand Up @@ -298,7 +299,8 @@ elseif (UNIX)
optimized CamcalbMatas${CMAKE_RELEASE_POSTFIX} debug CamcalbMatas${CMAKE_DEBUG_POSTFIX}
${OpenCV_LIBS}
optimized GraspCore${CMAKE_RELEASE_POSTFIX} debug GraspCore${CMAKE_DEBUG_POSTFIX}
optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX}
optimized GraspContact${CMAKE_RELEASE_POSTFIX} debug GraspContact${CMAKE_DEBUG_POSTFIX}
#optimized GraspGrasp${CMAKE_RELEASE_POSTFIX} debug GraspGrasp${CMAKE_DEBUG_POSTFIX}
optimized GraspApp${CMAKE_RELEASE_POSTFIX} debug GraspApp${CMAKE_DEBUG_POSTFIX}
optimized GraspActiveCtrl${CMAKE_RELEASE_POSTFIX} debug GraspActiveCtrl${CMAKE_DEBUG_POSTFIX}
optimized GraspArmHandForce${CMAKE_RELEASE_POSTFIX} debug GraspArmHandForce${CMAKE_DEBUG_POSTFIX}
Expand Down
19 changes: 19 additions & 0 deletions include/Spam/PosePlanner/PosePlanner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1025,6 +1025,25 @@ class PosePlanner : public grasp::Player {
/** Point selection */
// grasp::Cloud::PointSeqMap::iterator getPointsTrn(Data::Map::iterator dataPtr, golem::Mat34 &trn);

inline golem::Controller::State lookupState(golem::SecTmReal time = golem::SEC_TM_REAL_MAX) const {
golem::Controller::State dflt = controller->createState();
controller->setToDefault(dflt);

controller->lookupState(time, dflt);
dflt.cvel.setToDefault(info.getJoints());
dflt.cacc.setToDefault(info.getJoints());
return dflt;
}

inline golem::Controller::State lookupCommand(golem::SecTmReal time = golem::SEC_TM_REAL_MAX) const {
golem::Controller::State dflt = controller->createState();
controller->setToDefault(dflt);

controller->lookupCommand(time, dflt);
dflt.cvel.setToDefault(info.getJoints());
dflt.cacc.setToDefault(info.getJoints());
return dflt;
}

virtual void render() const;

Expand Down
2 changes: 1 addition & 1 deletion include/Spam/RagPlanner/RagPlanner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -388,7 +388,7 @@ class RagPlanner : public PosePlanner, protected golem::Profile::CallbackDist {
virtual void perform(const std::string& data, const std::string& item, const golem::Controller::State::Seq& trajectory, bool testTrajectory = true);

/** Plan and execute r2g, grasp, lift operations */
bool execute(grasp::data::Data::Map::iterator dataPtr, golem::Controller::State::Seq& trajectory);
bool execute(grasp::data::Data::Map::iterator dataPtr, grasp::Waypoint::Seq& trajectory);

virtual void render() const;
void renderHand(const golem::Controller::State &state, const golem::Bounds::Seq &bounds, bool clear = true);
Expand Down
2 changes: 1 addition & 1 deletion include/Spam/Spam/Belief.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,7 +241,7 @@ class Belief/* : public grasp::RBPose*/ {
/** Creates query object pose distribution */
void createQuery(const grasp::Cloud::PointSeq& points);
/** Creates a new set of poses (resampling wheel algorithm) */
virtual void createResample(const grasp::Manipulator::Pose& robotPose);
virtual void createResample(/*const grasp::Manipulator::Config& robotPose*/);
/** Creates belief update (on importance weights) given the robot's pose and the current belief state. NOTE: weights are normalised. */
// void createUpdate(const grasp::Manipulator *manipulator, const golem::Controller::State::Info handInfo, const golem::Waypoint &w, const FTGuard::Seq &triggeredGuards, const grasp::RealSeq &force);
/** Creates belief update (on importance weights) given the robot's pose and the current belief state. NOTE: weights are normalised. */
Expand Down
8 changes: 5 additions & 3 deletions include/Spam/Spam/Heuristic.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -383,7 +383,8 @@ class FTDrivenHeuristic : public golem::Heuristic {

/** Returns true only if expected collisions are likely to happen */
inline bool expectedCollisions(const golem::Controller::State& state) const {
return hypothesisBoundsSeq.empty() ? false : intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), hypothesisBoundsSeq, false);
const grasp::Manipulator::Config config = manipulator->getConfig(state);
return hypothesisBoundsSeq.empty() ? false : intersect(manipulator->getBounds(config.config, config.frame.toMat34()), hypothesisBoundsSeq, false);
//if (intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), pBelief->uncertaintyRegionBounds(), false) && !hypothesisBoundsSeq.empty()) {
// return intersect(manipulator->getBounds(manipulator->getConfig(state), manipulator->getPose(state).toMat34()), hypothesisBoundsSeq, false);
//for (auto i = pBelief->getHypotheses().begin(); i != pBelief->getHypotheses().end(); ++i)
Expand Down Expand Up @@ -491,8 +492,9 @@ class FTDrivenHeuristic : public golem::Heuristic {
golem::Real evaluate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint &w) const;

/** Estimate contact with hyptothesis */
inline golem::Real estimate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint &w) const {
return (*hypothesis)->estimate(ftDrivenDesc.evaluationDesc, manipulator->getPose(w.cpos), ftDrivenDesc.ftModelDesc.distMax);
inline golem::Real estimate(const Hypothesis::Seq::const_iterator &hypothesis, const golem::Waypoint& w) const {
const grasp::Manipulator::Config config(w.cpos);
return (*hypothesis)->estimate(ftDrivenDesc.evaluationDesc, config, ftDrivenDesc.ftModelDesc.distMax);
}

/** Penalises configurations which collide with the mean hypothesis */
Expand Down
81 changes: 44 additions & 37 deletions include/Spam/Spam/Spam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -245,7 +245,8 @@ class Collision {
typedef golem::_Mat34<_Real> Mat34;
typedef std::vector<Vec3> Vec3Seq;

typedef std::vector<_Bounds> Seq;
//typedef std::vector<_Bounds> Seq;
typedef golem::ScalarCoord<_Bounds, golem::Configspace> Coord;

/** Surface */
struct Surface {
Expand Down Expand Up @@ -646,40 +647,44 @@ class Collision {
virtual void create(golem::Rand& rand, const grasp::Cloud::PointSeq& points);

/** Collision detection at a given waypoint */
virtual bool check(const Waypoint& waypoint, const grasp::Manipulator::Pose& pose, bool debug = false) const;
virtual bool check(const Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const;
/** Collision detection using kdtree */
virtual bool check(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, bool debug = false) const;
virtual bool check(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, bool debug = false) const;

/** Collision detection for the observational model durin hypopthesis-based planning */
virtual golem::Real estimate(const FlannDesc& desc, const grasp::Manipulator::Pose& pose, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const;
virtual golem::Real estimate(const FlannDesc& desc, const grasp::Manipulator::Config& config, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const;
/** Collision detection using kdtree */
// virtual bool estimate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, bool debug = false) const;

/** Collision detection to simulate contact at execution time */
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, std::vector<golem::Configspace::Index>& joints, grasp::RealSeq& forces, bool debug = false) const;
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, std::vector<golem::Configspace::Index>& joints, grasp::RealSeq& forces, bool debug = false) const;
/** Collision detection to simulate contact at execution time */
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, FTGuard::Seq& joints, bool debug = false) const;
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, FTGuard::Seq& joints, bool debug = false) const;
/** Collision detection to simulate contact at execution time */
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, grasp::RealSeq& forces, bool debug = false) const;
virtual size_t simulate(const FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, grasp::RealSeq& forces, bool debug = false) const;

/** Collision likelihood estimation at a given waypoint */
virtual golem::Real evaluate(const Waypoint& waypoint, const grasp::Manipulator::Pose& pose, bool debug = false) const;
virtual golem::Real evaluate(const Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const;

/** Collision likelihood estimation at a given waypoint for belief update */
virtual golem::Real evaluate(const Waypoint& waypoint, const grasp::Manipulator::Pose& pose, const FTGuard::Seq& triggeredGuards, bool debug = false) const;
virtual golem::Real evaluate(const Waypoint& waypoint, const grasp::Manipulator::Config& config, const FTGuard::Seq& triggeredGuards, bool debug = false) const;

/** Collision likelihood estimation at a given waypoint for belief update */
virtual golem::Real evaluate(const FlannDesc& desc, const grasp::Manipulator::Pose& pose, FTGuard::Seq& triggeredGuards, bool debug = false) const;
virtual golem::Real evaluate(const FlannDesc& desc, const grasp::Manipulator::Config& config, FTGuard::Seq& triggeredGuards, bool debug = false) const;

/** Collision likelihood estimation at a given waypoint */
virtual golem::Real evaluate(const FlannDesc& desc, const grasp::Manipulator::Pose& pose, bool debug = false) const;
virtual golem::Real evaluate(const FlannDesc& desc, const grasp::Manipulator::Config& config, bool debug = false) const;

/** Collision likelihood estimation on a given path */
virtual golem::Real evaluate(const grasp::Manipulator::Waypoint::Seq& path, bool debug = false) const;

/** Bounds */
inline const Bounds::Seq& getBounds() const {
return bounds;
/** Joints bounds */
inline const Bounds::Coord& getJointBounds() const {
return jointBounds;
}
/** Base bounds */
inline const Bounds& getBaseBounds() const {
return baseBounds;
}
/** Points */
//inline const Bounds::Vec3Seq& getPoints() const {
Expand All @@ -691,15 +696,15 @@ class Collision {


/** Draw collisions */
void draw(const Waypoint& waypoint, const grasp::Manipulator::Pose& rbpose, golem::DebugRenderer& renderer) const;
void draw(const Waypoint& waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const;
/** Draw collisions with kdtree */
void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Pose& rbpose, const Collision::FlannDesc& desc) const;
void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config, const Collision::FlannDesc& desc) const;
/** Draw estimate */
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Pose& rbpose, const Collision::FlannDesc& desc) const;
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, const Collision::FlannDesc& desc) const;
/** Draw simulate */
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Pose& rbpose, std::vector<golem::Configspace::Index> &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const;
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, std::vector<golem::Configspace::Index> &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const;
/** Collision likelihood estimation at a given waypoint for belief update */
void draw(golem::DebugRenderer& renderer, const Waypoint& waypoint, const grasp::Manipulator::Pose& rbpose, const FTGuard::Seq &triggeredGuards, bool debug = false) const;
void draw(golem::DebugRenderer& renderer, const Waypoint& waypoint, const grasp::Manipulator::Config& config, const FTGuard::Seq &triggeredGuards, bool debug = false) const;

/** Returns ft_sensor desc in free space */
FTSensorDesc getFTBaseSensor() {
Expand All @@ -725,8 +730,10 @@ class Collision {
/** Description */
const Desc desc;

/** Joints + base */
Bounds::Seq bounds;
/** Joints */
Bounds::Coord jointBounds;
/** Base */
Bounds baseBounds;
/** Points */
Feature::Seq points;
// Bounds::Vec3Seq points;
Expand Down Expand Up @@ -890,26 +897,26 @@ class Hypothesis {
inline grasp::Cloud::PointSeq getCloud() const { return points; };

/** Collision detection at a given waypoint */
inline bool check(const Collision::Waypoint& waypoint, const grasp::Manipulator::Pose& pose, bool debug = false) const {
return collisionPtr->check(waypoint, pose, debug);
inline bool check(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const {
return collisionPtr->check(waypoint, config, debug);
};
/** Collision detection at a given waypoint */
inline bool check(const Collision::FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Pose& pose, bool debug = false) const {
return collisionPtr->check(desc, rand, pose, debug);
inline bool check(const Collision::FlannDesc& desc, const golem::Rand& rand, const grasp::Manipulator::Config& config, bool debug = false) const {
return collisionPtr->check(desc, rand, config, debug);
}

/** Collision detection at a given waypoint */
inline virtual golem::Real estimate(const Collision::FlannDesc& desc, const grasp::Manipulator::Pose& pose, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const {
return collisionPtr->estimate(desc, pose, maxDist, debug);
inline virtual golem::Real estimate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, golem::Real maxDist = golem::REAL_MAX, bool debug = false) const {
return collisionPtr->estimate(desc, config, maxDist, debug);
}

/** Collision likelihood estimation at a given waypoint */
inline golem::Real evaluate(const Collision::Waypoint& waypoint, const grasp::Manipulator::Pose& pose, bool debug = false) const {
return collisionPtr->evaluate(waypoint, pose, debug);
inline golem::Real evaluate(const Collision::Waypoint& waypoint, const grasp::Manipulator::Config& config, bool debug = false) const {
return collisionPtr->evaluate(waypoint, config, debug);
}
/** Collision likelihood estimation at a given waypoint */
inline golem::Real evaluate(const Collision::FlannDesc& desc, const grasp::Manipulator::Pose& pose, bool debug = false) const {
return collisionPtr->evaluate(desc, pose, debug);
inline golem::Real evaluate(const Collision::FlannDesc& desc, const grasp::Manipulator::Config& config, bool debug = false) const {
return collisionPtr->evaluate(desc, config, debug);
}

/** Return seq of bounds */
Expand All @@ -922,16 +929,16 @@ class Hypothesis {
void draw(golem::DebugRenderer &renderer) const;

/** Draw collisions */
void draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Pose& rbpose, golem::DebugRenderer& renderer) const;
void draw(const Collision::Waypoint &waypoint, const grasp::Manipulator::Config& config, golem::DebugRenderer& renderer) const;
/** Draw collision using kdtree */
void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Pose& rbpose) const;
void draw(golem::DebugRenderer& renderer, const golem::Rand& rand, const grasp::Manipulator::Config& config) const;
/** Draw estimate */
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Pose& rbpose, const Collision::FlannDesc& desc) const {
collisionPtr->draw(renderer, rbpose, desc);
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, const Collision::FlannDesc& desc) const {
collisionPtr->draw(renderer, config, desc);
}
/** Draw simulate */
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Pose& rbpose, std::vector<golem::Configspace::Index> &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const {
collisionPtr->draw(renderer, rbpose, joints, forces, desc);
void draw(golem::DebugRenderer& renderer, const grasp::Manipulator::Config& config, std::vector<golem::Configspace::Index> &joints, grasp::RealSeq &forces, const Collision::FlannDesc& desc) const {
collisionPtr->draw(renderer, config, joints, forces, desc);
}
Appearance appearance;

Expand Down
12 changes: 6 additions & 6 deletions resources/GraspDataContactModelSPAM.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,19 +4,19 @@
<data delete_if_moved="1" delete_if_unlinked="1" delete_if_temporary="1" model_suffix=".model">
<manipulator>
<config_map i1="0" i2="0" i3="0" i4="0" i5="0" i6="0" i7="0" i8="0" i9="0" i10="0" i11="10" i12="0" i13="0" i14="0" i15="14" i16="0" i17="0" i18="0" i19="18" i20="0" i21="0" i22="0" i23="22" i24="0" i25="0" i26="0" i27="26"/>
<trajectory lin="2000.0" ang="1000.0" collision="1" cluster_size="20" timeout="0.25"/>
<trajectory lin="1000.0" ang="1000.0" collision="1" cluster_size="20" timeout="0.1" throw="0"/>
</manipulator>

<configuration distance_scale="0.1" distance_stddev="1.0" distance_stddev_max="5.0" kernels="1000">
<configuration distance_scale="0.1" distance_stddev="1.0" distance_stddev_max="5.0" kernels="1000" transform_grad_dist="2.0">
<pose_stddev lin="0.002" ang="1000.0"/>
<config_stddev c1="0.15708" c2="0.15708" c3="0.15708" c4="0.15708" c5="0.15708" c6="0.15708" c7="0.15708" c8="0.015708" c9="0.015708" c10="0.015708" c11="0.015708" c12="0.015708" c13="0.015708" c14="0.015708" c15="0.015708" c16="0.015708" c17="0.015708" c18="0.015708" c19="0.015708" c20="0.015708" c21="0.015708" c22="0.015708" c23="0.015708" c24="0.015708" c25="0.015708" c26="0.015708" c27="0.015708"/>
</configuration>

<model id="Base">
<contact_3d curv_fac="100.0" min_num="50" subsample_size="10000" distance="0.02" lambda="20.0" normal_slope="3.1415927"/>
<contact_3d type="feature" curv_fac="100.0" min_num="50" subsample_size="10000" distance="0.02" lambda="20.0" normal_slope="3.1415927"/>
</model>
<model id="Any">
<contact_3d curv_fac="100.0" min_num="50" subsample_size="0" distance="0.01" lambda="50.0" normal_slope="3.1415927"/>
<contact_3d type="feature" curv_fac="100.0" min_num="50" subsample_size="0" distance="0.01" lambda="50.0" normal_slope="3.1415927"/>
</model>

<appearance>
Expand Down Expand Up @@ -57,9 +57,9 @@
<bounds_frame_size v1="0.01" v2="0.01" v3="0.01"/>
</contact>

<location>
<point>
<colour R="0" G="0" B="0" A="255"/>
</location>
</point>
</appearance>
</data>
</grasp>
Loading

0 comments on commit 8598cdc

Please sign in to comment.