Simox
2.3.74.0
|
Public Member Functions | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ApproachMovementSurfaceNormal (VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef, const std::string &graspPreshape="", float maxRetreatDist=0.0f, bool useFaceAreaDistribution=false) |
virtual | ~ApproachMovementSurfaceNormal () override |
Destructor. More... | |
Eigen::Matrix4f | createNewApproachPose () override |
Creates a new pose for approaching. More... | |
bool | getPositionOnObject (Eigen::Vector3f &storePos, Eigen::Vector3f &storeApproachDir) |
Returns a position with normal on the surface of the object. More... | |
virtual bool | setEEFToApproachPose (const Eigen::Vector3f &position, const Eigen::Vector3f &approachDir) |
Sets EEF to a position so that the Z component of the GCP coord system is aligned with -approachDir. More... | |
void | moveEEFAway (const Eigen::Vector3f &approachDir, float step, int maxLoops=1000) |
Eigen::Matrix4f | getEEFPose () |
bool | setEEFPose (const Eigen::Matrix4f &pose) |
Public Member Functions inherited from GraspStudio::ApproachMovementGenerator | |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ApproachMovementGenerator (VirtualRobot::SceneObjectPtr object, VirtualRobot::EndEffectorPtr eef, const std::string &graspPreshape="") |
virtual | ~ApproachMovementGenerator () |
destructor More... | |
virtual bool | setEEFToRandomApproachPose () |
Applies a random grasp hypothesis to the cloned EEF. More... | |
VirtualRobot::RobotPtr | getEEFRobotClone () |
This robot is moved around. More... | |
bool | setEEFPose (const Eigen::Matrix4f &pose) |
move EEF to pose (uses coord system of GCP) More... | |
bool | updateEEFPose (const Eigen::Matrix4f &deltaPose) |
update pose of EEF (GCP) More... | |
bool | updateEEFPose (const Eigen::Vector3f &deltaPosition) |
Eigen::Matrix4f | getEEFPose () |
get pose of EEF (coord system of GCP) More... | |
std::string | getGCPJoint () |
VirtualRobot::SceneObjectPtr | getObject () |
VirtualRobot::EndEffectorPtr | getEEF () |
VirtualRobot::EndEffectorPtr | getEEFOriginal () |
Eigen::Vector3f | getApproachDirGlobal () |
std::string | getName () |
virtual void | openHand () |
void | setVerbose (bool v) |
Protected Attributes | |
std::default_random_engine | randomEngine { std::random_device{}() } |
The random engine. More... | |
std::uniform_int_distribution< std::size_t > | distribUniform |
Uniform distribiton over face indices. More... | |
bool | useFaceAreasDistrib = false |
Indicates whether distribFaceAreas shall be used. More... | |
std::discrete_distribution< std::size_t > | distribFaceAreas |
std::uniform_real_distribution< float > | distribRetreatDistance |
Distribution to draw random retreat distances from. More... | |
Protected Attributes inherited from GraspStudio::ApproachMovementGenerator | |
VirtualRobot::SceneObjectPtr | object |
VirtualRobot::TriMeshModelPtr | objectModel |
VirtualRobot::EndEffectorPtr | eef |
Eigen::Vector3f | approachDirGlobal |
VirtualRobot::RobotPtr | eefRobot |
This robot is moved around. More... | |
VirtualRobot::EndEffectorPtr | eef_cloned |
std::string | name |
std::string | graspPreshape |
bool | verbose = false |
This class generates grasping configs by sampling a random surface position of the object and setting the EEF to a surface normal aligned position. The remaining free DoF (the rotation around the normal) is set randomly. Then the EEF is moved along the normal until a collision is detected or the GCP hits the object. If needed, the EEF is moved back until a collision-free pose is found.
Internally the EEF is cloned.
GraspStudio::ApproachMovementSurfaceNormal::ApproachMovementSurfaceNormal | ( | VirtualRobot::SceneObjectPtr | object, |
VirtualRobot::EndEffectorPtr | eef, | ||
const std::string & | graspPreshape = "" , |
||
float | maxRetreatDist = 0.0f , |
||
bool | useFaceAreaDistribution = false |
||
) |
To generate approach movements an object and an end effector has to be specified.
object | The object. |
eef | The end effector. |
graspPreshape | An optional preshape that can be used in order to "open" the eef. |
maxRandDist | If >0, the resulting apporach pose is randomly moved in the approach direction (away from the object) in order to create different distances to the object. |
usefaceAreaDistribution | If true, the probability of a face being selected is proportional to its area. If false, all faces are selected with equal probability. |
|
overridevirtualdefault |
Destructor.
|
overridevirtual |
Creates a new pose for approaching.
Implements GraspStudio::ApproachMovementGenerator.
Eigen::Matrix4f GraspStudio::ApproachMovementSurfaceNormal::getEEFPose | ( | ) |
bool GraspStudio::ApproachMovementSurfaceNormal::getPositionOnObject | ( | Eigen::Vector3f & | storePos, |
Eigen::Vector3f & | storeApproachDir | ||
) |
Returns a position with normal on the surface of the object.
void GraspStudio::ApproachMovementSurfaceNormal::moveEEFAway | ( | const Eigen::Vector3f & | approachDir, |
float | step, | ||
int | maxLoops = 1000 |
||
) |
bool GraspStudio::ApproachMovementSurfaceNormal::setEEFPose | ( | const Eigen::Matrix4f & | pose | ) |
|
virtual |
Sets EEF to a position so that the Z component of the GCP coord system is aligned with -approachDir.
|
protected |
Distribution with probability of a face proportional to its area. Only initialized if useFaceAreasDistrib is true.
|
protected |
Distribution to draw random retreat distances from.
|
protected |
Uniform distribiton over face indices.
|
protected |
The random engine.
|
protected |
Indicates whether distribFaceAreas shall be used.