rbdl_test/MotionValidator.cpp at master · hoergems/rbdl_test · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
#include "MotionValidator.hpp"

using std::cout;
using std::endl;

using namespace fcl;

namespace shared {

MotionValidator::MotionValidator(const ompl::base::SpaceInformationPtr &si,
                                 std::shared_ptr<Kinematics> kinematics,
                                 std::vector<std::shared_ptr<Obstacle> > obstacles,
                                 double delta_t,
                                 bool continuous_collision):
    ompl::base::MotionValidator(si),
    si_(si),
    kinematics_(kinematics),
    obstacles_(obstacles),
    delta_t_(delta_t),
    continuous_collision_(continuous_collision),
    utils_(),
    link_dimensions_()
{

}

bool MotionValidator::checkMotion(const std::vector<double> &s1,
                                  const std::vector<double> &s2,
                                  const bool &continuous_collision) const {
	/**for (size_t i = 0; i < s1.size(); i++) {
		if ((fabs((s2[i] - s1[i]) / delta_t_)) > max_joint_velocity_ + 0.00001) {
			return false;
		}
	}*/

    std::vector<OBB> manipulator_collision_structures_goal = utils_.createManipulatorCollisionStructures(s2,
                                                                                                         link_dimensions_,
                                                                                                         kinematics_);

    for (size_t i = 0; i < obstacles_.size(); i++) {
        if (!obstacles_[i]->isTraversable()) {
        	if (obstacles_[i]->in_collision(manipulator_collision_structures_goal)) {
        		return false;
        	}
        }
    }


    if (continuous_collision) {
        std::vector<fcl::CollisionObject> manipulator_collision_objects_start = utils_.createManipulatorCollisionObjects(s1,
                                                                                                                         link_dimensions_,
                                                                                                                         kinematics_);
        std::vector<fcl::CollisionObject> manipulator_collision_objects_goal = utils_.createManipulatorCollisionObjects(s2,
                                                                                                                        link_dimensions_,
                                                                                                                        kinematics_);
        for (size_t i = 0; i < obstacles_.size(); i++) {
            if (!obstacles_[i]->isTraversable()) {
                for (size_t j = 0; j < manipulator_collision_objects_start.size(); j++) {
                	if (obstacles_[i]->in_collision(manipulator_collision_objects_start[j], manipulator_collision_objects_goal[j])) {
                		return false;
                	}
                }
            }
        }
    }

    return true;
}

/** Check if a motion between two states is valid. This assumes that state s1 is valid */
bool MotionValidator::checkMotion(const ompl::base::State *s1, const ompl::base::State *s2) const {
    std::vector<double> angles1;
    std::vector<double> angles2;
    for (unsigned int i = 0; i < si_->getStateSpace()->getDimension(); i++) {
        angles1.push_back(s1->as<ompl::base::RealVectorStateSpace::StateType>()->values[i]);
        angles2.push_back(s2->as<ompl::base::RealVectorStateSpace::StateType>()->values[i]);
    }

    return checkMotion(angles1, angles2, continuous_collision_);
}

/** Check if a motion between two states is valid. This assumes that state s1 is valid */
bool MotionValidator::checkMotion(const ompl::base::State *s1,
                                  const ompl::base::State *s2,
                                  std::pair< ompl::base::State *, double > &/*lastValid*/) const {
    return checkMotion(s1, s2);
}

bool MotionValidator::isValid(const std::vector<double> &s1) const {
    std::vector<OBB> manipulator_collision_structures = utils_.createManipulatorCollisionStructures(s1,
                                                                                                    link_dimensions_,
                                                                                                    kinematics_);
    for (size_t i = 0; i < obstacles_.size(); i++) {
        if (!obstacles_[i]->getTerrain()->isTraversable()) {
        	if (obstacles_[i]->in_collision(manipulator_collision_structures)) {
        		return false;
        	}
        }
    }
    return true;
}

void MotionValidator::setObstacles(std::vector<std::shared_ptr<Obstacle> > &obstacles) {
    obstacles_.clear();
    for (size_t i = 0; i < obstacles.size(); i++) {
        obstacles_.push_back(obstacles[i]);
    }
}

void MotionValidator::setLinkDimensions(std::vector<std::vector<double>> &link_dimensions) {
    for (size_t i = 0; i < link_dimensions.size(); i++) {
        link_dimensions_.push_back(link_dimensions[i]);
    }
}
}