/*! 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; }