/*!
Copies the point and dir pointers from \a p. Also copies the values
of empty and gdType .
*/
cartesianGraspDirection::cartesianGraspDirection(const cartesianGraspDirection& p) : GraspDirection()
{
point = new cartesian_coordinates(p.get_point());
dir = new cartesian_coordinates(p.get_dir());
empty = p.get_empty();
set_gdType(p.get_gdType());
}
/*!
Returns a normalized distance measure between this grasp direction and
\a to .
*/
double
cartesianGraspDirection::distanceTo(cartesianGraspDirection to) const
{
double dist = point->distanceTo(to.get_point());
dist += dir->distanceTo(to.get_dir());
/* max is 2.0, every dist normed to 1 */
return dist / 2.0;
}
/*!
Moves the hand to a position that is 1 meter away from its current
position in the direction of the hand approach vector \a gd. Since
the hand's moveTo function is used, the motion will stop if it encounters
any obstructions, i.e. the object to be grasped, along the way. If
\a render_in is TRUE, the result of this motion is rendered.
*/
bool
grasp_tester::moveIt(cartesianGraspDirection gd, bool render_in)
{
transf to(my_hand->getTran().rotation(),
my_hand->getTran().translation() + 1000 * gd.get_dir());
my_hand->moveTo(to, 50 * Contact::THRESHOLD, M_PI / 36.0);
if (render_in) {
myViewer->render();
}
return true;
}
