problem.impl.hh

Go to the documentation of this file.

// Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, JRL.
//
 
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
// DAMAGE.
//
// This software is provided "as is" without warranty of any kind,
// either expressed or implied, including but not limited to the
// implied warranties of fitness for a particular purpose.
//
// See the COPYING file for more information.
 
#ifndef HPP_CORBASERVER_PROBLEM_IMPL_HH
#define HPP_CORBASERVER_PROBLEM_IMPL_HH
#include <stdlib.h>
 
#include <vector>
 
#include "hpp/corbaserver/deprecated.hh"
#include "hpp/corbaserver/fwd.hh"
#include "hpp/corbaserver/problem-idl.hh"
#include "hpp/corbaserver/problem-solver-map.hh"
#include "hpp/core_idl/_problem-idl.hh"
#include "hpp/core_idl/distances-idl.hh"
 
namespace hpp {
namespace corbaServer {
namespace impl {
using CORBA::Any;
using CORBA::Boolean;
using CORBA::Long;
using CORBA::ULong;
using CORBA::UShort;
 
class Problem : public virtual POA_hpp::corbaserver::Problem {
 public:
  Problem();
 
  void setServer(ServerPlugin* server) { server_ = server; }
 
  virtual Names_t* getAvailable(const char* what);
 
  virtual Names_t* getSelected(const char* what);
 
  virtual void setParameter(const char* name, const Any& value);
 
  virtual Any* getParameter(const char* name);
 
  virtual char* getParameterDoc(const char* name);
 
  virtual bool selectProblem(const char* problemName);
 
  virtual void resetProblem();
 
  virtual bool loadPlugin(const char* pluginName);
 
  virtual void movePathToProblem(ULong pathId, const char* problemName,
                                 const Names_t& jointNames);
 
  virtual void setMaxNumThreads(UShort n);
 
  virtual UShort getMaxNumThreads();
 
  virtual void setRandomSeed(const Long seed) { srand((int)seed); }
 
  virtual void setInitialConfig(const hpp::floatSeq& dofArray);
 
  virtual hpp::floatSeq* getInitialConfig();
 
  virtual void addGoalConfig(const hpp::floatSeq& dofArray);
 
  virtual hpp::floatSeqSeq* getGoalConfigs();
 
  virtual void resetGoalConfigs();
 
  void setGoalConstraints(const Names_t& constraints);
 
  void resetGoalConstraints();
  virtual void createOrientationConstraint(const char* constraintName,
                                           const char* joint1Name,
                                           const char* joint2Name,
                                           const Double* p,
                                           const hpp::boolSeq& mask);
 
  virtual void createTransformationConstraint(const char* constraintName,
                                              const char* joint1Name,
                                              const char* joint2Name,
                                              const Transform_ p,
                                              const hpp::boolSeq& mask);
 
  virtual void createTransformationConstraint2(const char* constraintName,
                                               const char* joint1Name,
                                               const char* joint2Name,
                                               const Transform_ frame1,
                                               const Transform_ frame2,
                                               const hpp::boolSeq& mask);
 
  virtual void createTransformationR3xSO3Constraint(const char* constraintName,
                                                    const char* joint1Name,
                                                    const char* joint2Name,
                                                    const Transform_ frame1,
                                                    const Transform_ frame2,
                                                    const hpp::boolSeq& mask);
 
  virtual void createLockedJoint(const char* lockedJointName,
                                 const char* jointName,
                                 const hpp::floatSeq& value);
 
  virtual void createLockedJointWithComp(const char* lockedJointName,
                                         const char* jointName,
                                         const hpp::floatSeq& value,
                                         const hpp::ComparisonTypes_t& comp);
 
  virtual void createLockedExtraDof(const char* lockedDofName, ULong index,
                                    const hpp::floatSeq& value);
 
  virtual void createManipulability(const char* name, const char* function);
 
  void createRelativeComConstraint(const char* constraintName, const char* comn,
                                   const char* jointName, const floatSeq& point,
                                   const hpp::boolSeq& mask);
 
  void createComBeetweenFeet(const char* constraintName, const char* comn,
                             const char* jointLName, const char* jointRName,
                             const floatSeq& pointL, const floatSeq& pointR,
                             const char* jointRefName, const floatSeq& pRef,
                             const hpp::boolSeq& mask);
 
  virtual void createConvexShapeContactConstraint(
      const char* constraintName, const Names_t& floorJoints,
      const Names_t& objectJoints, const hpp::floatSeqSeq& points,
      const hpp::intSeqSeq& objTriangles, const hpp::intSeqSeq& floorTriangles);
 
  virtual void createPositionConstraint(const char* constraintName,
                                        const char* joint1Name,
                                        const char* joint2Name,
                                        const hpp::floatSeq& point1,
                                        const hpp::floatSeq& point2,
                                        const hpp::boolSeq& mask);
 
  virtual void createConfigurationConstraint(const char* constraintName,
                                             const hpp::floatSeq& goal,
                                             const hpp::floatSeq& weights);
 
  virtual void createDistanceBetweenJointConstraint(const char* constraintName,
                                                    const char* joint1Name,
                                                    const char* joint2Name,
                                                    Double distance);
 
  virtual void createDistanceBetweenJointAndObjects(const char* constraintName,
                                                    const char* joint1Name,
                                                    const hpp::Names_t& objects,
                                                    Double distance);
 
  virtual void createIdentityConstraint(const char* constraintName,
                                        const Names_t& inJoints,
                                        const hpp::Names_t& outJoints);
 
  virtual bool applyConstraints(const hpp::floatSeq& input,
                                hpp::floatSeq_out output,
                                Double& residualError);
 
  virtual bool optimize(const hpp::floatSeq& input, hpp::floatSeq_out output,
                        hpp::floatSeq_out residualError);
 
  virtual void computeValueAndJacobian(const hpp::floatSeq& config,
                                       hpp::floatSeq_out value,
                                       hpp::floatSeqSeq_out jacobian);
 
  virtual bool generateValidConfig(ULong maxIter, hpp::floatSeq_out output,
                                   Double& residualError);
 
  virtual void addPassiveDofs(const char* constraintName,
                              const hpp::Names_t& dofName);
 
  virtual void getConstraintDimensions(const char* constraintName,
                                       ULong& inputSize,
                                       ULong& inputDerivativeSize,
                                       ULong& outputSize,
                                       ULong& outputDerivativeSize);
 
  virtual void setConstantRightHandSide(const char* constraintName,
                                        CORBA::Boolean constant);
 
  virtual bool getConstantRightHandSide(const char* constraintName);
 
  virtual floatSeq* getRightHandSide();
 
  virtual void setRightHandSide(const hpp::floatSeq& rhs);
 
  virtual void setRightHandSideFromConfig(const hpp::floatSeq& config);
 
  virtual void setRightHandSideByName(const char* constraintName,
                                      const hpp::floatSeq& rhs);
 
  virtual void setRightHandSideFromConfigByName(const char* constraintName,
                                                const hpp::floatSeq& config);
 
  virtual void resetConstraints();
  virtual void resetConstraintMap();
  virtual void addNumericalConstraints(const char* constraintName,
                                       const hpp::Names_t& constraintNames,
                                       const hpp::intSeq& priorities);
  virtual void setNumericalConstraintsLastPriorityOptional(const bool optional);
 
  virtual void addLockedJointConstraints(const char* configProjName,
                                         const hpp::Names_t& lockedJointNames);
 
  virtual char* displayConstraints();
 
  virtual void filterCollisionPairs();
  virtual Double getErrorThreshold();
  virtual void setErrorThreshold(Double threshold);
  virtual void setDefaultLineSearchType(const char* type);
  virtual ULong getMaxIterProjection();
  virtual void setMaxIterProjection(ULong iterations);
  virtual ULong getMaxIterPathPlanning();
  virtual void setMaxIterPathPlanning(ULong iterations);
  virtual void setTimeOutPathPlanning(double timeOut);
  virtual double getTimeOutPathPlanning();
 
  virtual void addPathOptimizer(const char* pathOptimizerType);
 
  virtual void clearPathOptimizers();
 
  virtual void addConfigValidation(const char* configValidationType);
 
  virtual void clearConfigValidations();
 
  virtual void selectPathValidation(const char* pathValidationType,
                                    Double tolerance);
 
  virtual void selectPathProjector(const char* pathProjectorType,
                                   Double tolerance);
 
  virtual void selectPathPlanner(const char* pathPlannerType);
 
  virtual void selectDistance(const char* distanceType);
 
  virtual void selectSteeringMethod(const char* steeringMethodType);
 
  virtual void selectConfigurationShooter(const char* configurationShooterType);
 
  virtual bool prepareSolveStepByStep();
  virtual bool executeOneStep();
  virtual void finishSolveStepByStep();
 
  virtual hpp::intSeq* solve();
 
  virtual bool directPath(const hpp::floatSeq& startConfig,
                          const hpp::floatSeq& endConfig,
                          CORBA::Boolean validate, ULong& pathId,
                          CORBA::String_out report);
 
  virtual bool reversePath(ULong pathId, ULong& reversedPathId);
 
  virtual void addConfigToRoadmap(const hpp::floatSeq& config);
 
  virtual void addEdgeToRoadmap(const hpp::floatSeq& config1,
                                const hpp::floatSeq& config2, ULong pathId,
                                bool bothEdges);
 
  virtual void appendDirectPath(ULong pathId, const hpp::floatSeq& config,
                                Boolean validate);
 
  virtual void concatenatePath(ULong startId, ULong endId);
 
  virtual void extractPath(ULong pathId, Double start, Double end);
 
  virtual void erasePath(ULong pathId);
 
  virtual bool projectPath(ULong pathId);
 
  virtual void interruptPathPlanning();
 
  virtual Long numberPaths();
 
  virtual hpp::intSeq* optimizePath(ULong pathId);
 
  virtual Double pathLength(ULong pathId);
 
  virtual hpp::floatSeq* configAtParam(ULong pathId, Double atDistance);
 
  virtual hpp::floatSeq* derivativeAtParam(ULong pathId, ULong order,
                                           Double atDistance);
 
  virtual hpp::floatSeqSeq* getWaypoints(ULong inPathId, floatSeq_out times);
  virtual hpp::floatSeqSeq* nodes();
  virtual Long numberEdges();
  virtual void edge(ULong edgeId, hpp::floatSeq_out q1, hpp::floatSeq_out q2);
  virtual Long connectedComponentOfEdge(ULong edgeId);
  virtual hpp::floatSeq* node(ULong nodeId);
  virtual Long connectedComponentOfNode(ULong nodeId);
  virtual Long numberNodes();
  virtual Long numberConnectedComponents();
  virtual hpp::floatSeqSeq* nodesConnectedComponent(ULong connectedComponentId);
 
  virtual hpp::floatSeq* getNearestConfig(const hpp::floatSeq& config,
                                          const Long connectedComponentId,
                                          hpp::core::value_type& distance);
 
  virtual void clearRoadmap();
  virtual void resetRoadmap();
  virtual void saveRoadmap(const char* filename);
  virtual void loadRoadmap(const char* filename);
 
  virtual void savePath(hpp::core_idl::Path_ptr path, const char* filename);
  virtual hpp::core_idl::Path_ptr loadPath(const char* filename);
 
  virtual void scCreateScalarMultiply(const char* outName, Double scalar,
                                      const char* inName);
 
  hpp::core_idl::Distance_ptr getDistance();
 
  void setDistance(hpp::core_idl::Distance_ptr distance);
 
  hpp::core_idl::Path_ptr getPath(ULong pathId);
 
  ULong addPath(hpp::core_idl::PathVector_ptr _path);
 
  hpp::core_idl::SteeringMethod_ptr getSteeringMethod();
 
  hpp::core_idl::PathValidation_ptr getPathValidation();
 
  hpp::core_idl::PathPlanner_ptr getPathPlanner();
 
  hpp::core_idl::Problem_ptr getProblem();
 
  hpp::constraints_idl::Implicit_ptr getConstraint(const char* name);
 
  void setRobot(hpp::pinocchio_idl::Device_ptr robot);
 
  pinocchio_idl::CollisionObject_ptr getObstacle(const char* name);
 
  core_idl::Problem_ptr createProblem(pinocchio_idl::Device_ptr robot);
 
  core_idl::Roadmap_ptr createRoadmap(core_idl::Distance_ptr distance,
                                      pinocchio_idl::Device_ptr robot);
  core_idl::Roadmap_ptr readRoadmap(const char* filename,
                                    pinocchio_idl::Device_ptr robot);
  void writeRoadmap(const char* filename, core_idl::Roadmap_ptr roadmap,
                    pinocchio_idl::Device_ptr robot);
  core_idl::PathPlanner_ptr createPathPlanner(const char* type,
                                              core_idl::Problem_ptr _problem,
                                              core_idl::Roadmap_ptr roadmap);
  core_idl::PathOptimizer_ptr createPathOptimizer(
      const char* type, core_idl::Problem_ptr _problem);
 
  core_idl::PathProjector_ptr createPathProjector(const char* type,
                                                  core_idl::Problem_ptr robot,
                                                  value_type parameter);
  core_idl::PathValidation_ptr createPathValidation(
      const char* type, pinocchio_idl::Device_ptr robot, value_type parameter);
  core_idl::ConfigValidation_ptr createConfigValidation(
      const char* type, pinocchio_idl::Device_ptr robot) override;
  core_idl::ConfigurationShooter_ptr createConfigurationShooter(
      const char* type, core_idl::Problem_ptr _problem);
  core_idl::Distance_ptr createDistance(const char* type,
                                        core_idl::Problem_ptr _problem);
  core_idl::SteeringMethod_ptr createSteeringMethod(
      const char* type, core_idl::Problem_ptr _problem);
 
  core_idl::Constraint_ptr createConstraintSet(pinocchio_idl::Device_ptr robot,
                                               const char* name);
  core_idl::Constraint_ptr createConfigProjector(
      pinocchio_idl::Device_ptr robot, const char* name, Double threshold,
      ULong iterations);
 
 private:
  core::ProblemSolverPtr_t problemSolver();
  ServerPlugin* server_;
};
}  // end of namespace impl.
}  // end of namespace corbaServer.
}  // end of namespace hpp.
 
#endif