Newer
Older
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
// Copyright (c) 2019 CNRS
// Authors: Joseph Mirabel
//
// This file is part of hpp-manipulation
// hpp-manipulation is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-manipulation is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-manipulation If not, see
// <http://www.gnu.org/licenses/>.
# include <hpp/manipulation/path-planner/end-effector-trajectory.hh>
# include <pinocchio/multibody/data.hpp>
# include <hpp/util/exception-factory.hh>
# include <hpp/pinocchio/util.hh>
# include <hpp/pinocchio/device-sync.hh>
# include <hpp/pinocchio/liegroup-element.hh>
# include <hpp/constraints/differentiable-function.hh>
# include <hpp/constraints/implicit.hh>
# include <hpp/core/config-projector.hh>
# include <hpp/core/config-validations.hh>
# include <hpp/core/configuration-shooter.hh>
# include <hpp/core/path-validation.hh>
# include <hpp/core/problem.hh>
# include <hpp/core/roadmap.hh>
# include <hpp/manipulation/steering-method/end-effector-trajectory.hh>
namespace hpp {
namespace manipulation {
namespace pathPlanner {
typedef manipulation::steeringMethod::EndEffectorTrajectory SM_t;
typedef manipulation::steeringMethod::EndEffectorTrajectoryPtr_t SMPtr_t;
EndEffectorTrajectoryPtr_t EndEffectorTrajectory::create
(const core::ProblemConstPtr_t& problem)
{
EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem));
ptr->init(ptr);
return ptr;
}
EndEffectorTrajectoryPtr_t EndEffectorTrajectory::createWithRoadmap (
const core::ProblemConstPtr_t& problem,
const core::RoadmapPtr_t& roadmap)
EndEffectorTrajectoryPtr_t ptr (new EndEffectorTrajectory(problem,
roadmap));
ptr->init(ptr);
return ptr;
}
void EndEffectorTrajectory::tryConnectInitAndGoals ()
{}
void EndEffectorTrajectory::startSolve ()
{
//core::PathPlanner::startSolve();
//problem().checkProblem ();
std::string msg ("No device in problem.");
hppDout (error, msg);
throw std::runtime_error (msg);
}
std::string msg ("No init config in problem.");
hppDout (error, msg);
throw std::runtime_error (msg);
}
// Tag init and goal configurations in the roadmap
roadmap()->resetGoalNodes ();
SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
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
116
117
118
119
120
121
122
123
124
if (!sm)
throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
if (!sm->constraints() || !sm->constraints()->configProjector())
throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
const constraints::ImplicitPtr_t& trajConstraint = sm->trajectoryConstraint ();
if (!trajConstraint)
throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
if (!sm->trajectory())
throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
const core::NumericalConstraints_t& ncs = constraints->numericalConstraints();
bool ok = false;
for (std::size_t i = 0; i < ncs.size(); ++i) {
if (ncs[i] == trajConstraint) {
ok = true;
break; // Same pointer
}
// Here, we do not check the right hand side on purpose.
// if (*ncs[i] == *trajConstraint) {
if (ncs[i]->functionPtr() == trajConstraint->functionPtr()
&& ncs[i]->comparisonType() == trajConstraint->comparisonType()) {
ok = true;
// TODO We should only modify the path constraint.
// However, only the pointers to implicit constraints are copied
// while we would like the implicit constraints to be copied as well.
ncs[i]->rightHandSideFunction (sm->trajectory());
break; // logically identical
}
}
if (!ok) {
HPP_THROW (std::logic_error, "EndEffectorTrajectory could not find "
"constraint " << trajConstraint->function());
}
}
void EndEffectorTrajectory::oneStep ()
{
SMPtr_t sm (HPP_DYNAMIC_PTR_CAST (SM_t, problem()->steeringMethod()));
if (!sm)
throw std::invalid_argument ("Steering method must be of type hpp::manipulation::steeringMethod::EndEffectorTrajectory");
if (!sm->trajectoryConstraint ())
throw std::invalid_argument ("EndEffectorTrajectory has no trajectory constraint.");
if (!sm->trajectory())
throw std::invalid_argument ("EndEffectorTrajectory has no trajectory.");
if (!sm->constraints() || !sm->constraints()->configProjector())
throw std::invalid_argument ("Steering method constraint has no ConfigProjector.");
core::ConfigProjectorPtr_t constraints (sm->constraints()->configProjector());
core::ConfigValidationPtr_t cfgValidation (problem()->configValidations());
core:: PathValidationPtr_t pathValidation (problem()->pathValidation());
core:: ValidationReportPtr_t cfgReport;
core::PathValidationReportPtr_t pathReport;
core::interval_t timeRange (sm->timeRange());
std::vector<core::Configuration_t> qs (configurations(*problem()->initConfig()));
if (qs.empty()) {
hppDout (info, "Failed to generate initial configs.");
return;
}
// Generate a valid initial configuration.
bool success = false;
bool resetRightHandSide = true;
std::size_t i;
vector_t times (nDiscreteSteps_+1);
matrix_t steps (problem()->robot()->configSize(), nDiscreteSteps_+1);
times[0] = timeRange.first;
for (int j = 1; j < nDiscreteSteps_; ++j)
times[j] = timeRange.first + j * (timeRange.second - timeRange.first) / nDiscreteSteps_;
times[nDiscreteSteps_] = timeRange.second;
for (i = 0; i < qs.size(); ++i)
{
if (resetRightHandSide) {
constraints->rightHandSideAt (times[0]);
resetRightHandSide = false;
}
Configuration_t& q (qs[i]);
if (!constraints->apply (q)) continue;
if (!cfgValidation->validate (q, cfgReport)) continue;
resetRightHandSide = true;
steps.col(0) = q;
success = true;
for (int j = 1; j <= nDiscreteSteps_; ++j) {
constraints->rightHandSideAt (times[j]);
hppDout (info, "RHS: " << setpyformat << constraints->rightHandSide().transpose());
steps.col(j) = steps.col(j-1);
if (!constraints->apply (steps.col(j))) {
hppDout (info, "Failed to generate destination config.\n" << setpyformat
<< *constraints
<< "\nq=" << one_line (q));
success = false;
break;
}
}
if (!success) continue;
success = false;
if (!cfgValidation->validate (steps.col(nDiscreteSteps_), cfgReport)) {
hppDout (info, "Destination config is in collision.");
continue;
}
core::PathPtr_t path = sm->projectedPath(times, steps);
if (!path) {
hppDout (info, "Steering method failed.\n" << setpyformat
<< "times: " << one_line(times) << '\n'
<< "configs:\n" << condensed(steps.transpose()) << '\n'
);
continue;
}
core::PathPtr_t validPart;
if (!pathValidation->validate (path, false, validPart, pathReport)) {
hppDout (info, "Path is in collision.");
continue;
}
roadmap()->initNode (make_shared<Configuration_t>(steps.col(0)));
core::NodePtr_t init = roadmap()-> initNode ();
core::NodePtr_t goal = roadmap()->addGoalNode (
make_shared<Configuration_t>(steps.col(nDiscreteSteps_)));
roadmap()->addEdge (init, goal, path);
success = true;
if (feasibilityOnly_) break;
}
}
std::vector<core::Configuration_t> EndEffectorTrajectory::configurations(const core::Configuration_t& q_init)
{
if (!ikSolverInit_) {
std::vector<core::Configuration_t> configs(nRandomConfig_ + 1);
configs[0] = q_init;
for (int i = 1; i < nRandomConfig_ + 1; ++i)
problem()->configurationShooter()->shoot(configs[i]);
return configs;
}
// TODO Compute the target and call ikSolverInit_
// See https://gepgitlab.laas.fr/airbus-xtct/hpp_airbus_xtct for an
// example using IKFast.
throw std::runtime_error ("Using an IkSolverInitialization is not implemented yet");
}
EndEffectorTrajectory::EndEffectorTrajectory
(const core::ProblemConstPtr_t& problem) : core::PathPlanner (problem)
EndEffectorTrajectory::EndEffectorTrajectory
(const core::ProblemConstPtr_t& problem,
const core::RoadmapPtr_t& roadmap)
: core::PathPlanner (problem, roadmap)
{}
void EndEffectorTrajectory::checkFeasibilityOnly (bool enable)
{
feasibilityOnly_ = enable;
}
void EndEffectorTrajectory::init (const EndEffectorTrajectoryWkPtr_t& weak)
{
core::PathPlanner::init (weak);
weak_ = weak;
nRandomConfig_ = 10;
nDiscreteSteps_ = 1;
feasibilityOnly_ = true;
}
} // namespace pathPlanner
} // namespace manipulation
} // namespace hpp