Commit da33ffed authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Reuse types from hpp-corbaserver IDL and homogenize checks of input conf

parent 778c3ad7
......@@ -20,16 +20,9 @@
#ifndef HPP_WHOLEBODY_STEP_CORBA_IDL
# define HPP_WHOLEBODY_STEP_CORBA_IDL
module hpp {
/// Corba exception travelling through the Corba channel
exception Error
{
string msg;
};
/// Robot configuration is defined by a sequence of dof value.
typedef sequence<double> dofSeq;
typedef sequence<dofSeq> dofSeqSeq;
# include <hpp/corbaserver/common.idl>
module hpp {
module corbaserver {
module wholebody_step {
interface Problem {
......@@ -66,7 +59,7 @@ module hpp {
/// Create static stability constraints and store into ProblemSolver
/// \param prefix prefix of the names of the constraint as stored in
/// core::ProblemSolver,
/// \param dofArray input configuration for computing constraint reference,
/// \param floatArray input configuration for computing constraint reference,
/// \param leftAnkle, rightAnkle: names of the ankle joints.
/// \param comName name of the PartialCOM in the problem solver map. Put "" for
/// a full COM computations.
......@@ -76,7 +69,7 @@ module hpp {
/// and are accessible through the method
/// core::ProblemSolver::addNumericalConstraint:
void addStaticStabilityConstraints (in string prefix,
in dofSeq dofArray,
in floatSeq floatArray,
in string leftAnkle,
in string rightAnkle,
in string comName,
......
......@@ -22,7 +22,9 @@ SET(IDL_SOURCES
FINDPYTHON(2.7 EXACT REQUIRED)
INCLUDE_DIRECTORIES(${CMAKE_BINARY_DIR}/src)
OMNIIDL_INCLUDE_DIRECTORIES(${CMAKE_SOURCE_DIR}/idl)
OMNIIDL_INCLUDE_DIRECTORIES(
${HPP_CORBASERVER_DATAROOTDIR}/idl ${CMAKE_SOURCE_DIR}/idl
)
FILE(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/hpp/corbaserver/wholebody-step)
FOREACH(IDL ${IDL_SOURCES})
......
......@@ -21,10 +21,15 @@
#include <cassert>
#include <hpp/util/debug.hh>
#include <hpp/pinocchio/center-of-mass-computation.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/core/constraint-set.hh>
#include <hpp/core/problem-solver.hh>
#include <hpp/corbaserver/conversions.hh>
#include <hpp/wholebody-step/static-stability-constraint.hh>
#include <hpp/corbaserver/wholebody-step/server.hh>
......@@ -36,6 +41,7 @@ namespace hpp {
using hpp::core::ConstraintSetPtr_t;
using hpp::core::ConfigProjectorPtr_t;
using hpp::pinocchio::CenterOfMassComputation;
using hpp::corbaServer::floatSeqToConfigPtr;
namespace impl {
namespace {
......@@ -126,32 +132,6 @@ namespace hpp {
}
}
static ConfigurationPtr_t dofSeqToConfig
(ProblemSolverPtr_t problemSolver, const hpp::dofSeq& dofArray)
{
CORBA::ULong configDim = dofArray.length();
ConfigurationPtr_t config (new Configuration_t (configDim));
// Get robot in hppPlanner object.
DevicePtr_t robot = problemSolver->robot ();
// Compare size of input array with number of degrees of freedom of
// robot.
if (configDim != robot->configSize ()) {
hppDout (error, "robot configSize (" << robot->configSize ()
<< ") is different from config size ("
<< configDim << ")");
throw std::runtime_error
("robot nb dof is different from config size");
}
// Fill dof vector with dof array.
for (CORBA::ULong iDof=0; iDof < configDim; ++iDof) {
(*config) [iDof] = dofArray [iDof];
}
return config;
}
Problem::Problem () : server_ (0x0) {}
ProblemSolverPtr_t Problem::problemSolver ()
......@@ -160,7 +140,7 @@ namespace hpp {
}
void Problem::addStaticStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
(const char* prefix, const hpp::floatSeq& dofArray,
const char* leftAnkle, const char* rightAnkle, const char* comName,
const StaticStabilityType type)
throw (hpp::Error)
......@@ -168,12 +148,12 @@ namespace hpp {
using pinocchio::CenterOfMassComputationPtr_t;
using core::DifferentiableFunctionPtr_t;
try {
ConfigurationPtr_t config = dofSeqToConfig (problemSolver(), dofArray);
const DevicePtr_t& robot (problemSolver()->robot ());
if (!robot) {
throw Error ("You should set the robot before defining"
" constraints.");
}
ConfigurationPtr_t config = floatSeqToConfigPtr (robot, dofArray, true);
JointPtr_t la = robot->getJointByName (leftAnkle);
JointPtr_t ra = robot->getJointByName (rightAnkle);
std::string comN (comName);
......
......@@ -42,7 +42,7 @@ namespace hpp {
}
virtual void addStaticStabilityConstraints
(const char* prefix, const hpp::dofSeq& dofArray,
(const char* prefix, const hpp::floatSeq& floatArray,
const char* leftAnkle, const char* rightAnkle, const char* comName,
const StaticStabilityType type)
throw (hpp::Error);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment