Commit fa51596e authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added cone accessors and optimization method

parent ce31ce49
......@@ -225,6 +225,32 @@ module hpp
/// \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
floatSeqSeq interpolateConfigs(in floatSeqSeq configs, in double robustnessTreshold) raises (Error);
/// returns the CWC of the robot at a given state
///
/// \param stateId The considered state
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactCone(in unsigned short stateId) raises (Error);
/// returns the CWC of the robot between two states
///
/// \param stateId The considered state
/// \return H matrix, such that H w <= h. h is added as the last column
floatSeqSeq getContactIntermediateCone(in unsigned short stateId) raises (Error);
/// Create a path for the root given
/// a set of 3d key points
/// The path is composed of n+1 linear interpolations
/// between the n positions.
/// The rotation is linearly interpolated as well,
/// between a start and a goal rotation. The resulting path
/// is added to the problem solver
/// \param positions array of positions
/// \param q1 quaternion of 1st rotation
/// \param q2 quaternion of 2nd rotation
void generateRootPath(in floatSeqSeq rootPositions, in floatSeq q1, in floatSeq q2) raises (Error);
/// Provided a path has already been computed and interpolated, generate a continuous path
/// between two indicated states. The states do not need to be consecutive, but increasing in Id.
/// Will fail if the index of the states do not exist
......
......@@ -19,6 +19,7 @@
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
import hpp.gepetto.blender.exportmotion as em
from numpy import array
## Corba clients to the various servers
#
......@@ -234,13 +235,11 @@ class FullBody (object):
def interpolate(self, stepsize, pathId = 1, robustnessTreshold = 0):
return self.client.rbprm.rbprm.interpolate(stepsize, pathId, robustnessTreshold)
## Discretizes a path return by a motion planner into a discrete
# sequence of balanced, contact configurations and returns
# the sequence as an array of configurations
#
# \param stateId id of the first state
# \param pathId Id of the path to compute from
# \param robustnessTreshold minimum value of the static equilibrium robustness criterion required to accept the configuration (0 by default).
## Provided a discrete contact sequence has already been computed, computes
# all the contact positions and normals for a given state, the next one, and the intermediate between them.
#
# \param stateId normalized step for generation along the path (ie the path has a length of 1).
# \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
def computeContactPoints(self, stateId):
rawdata = self.client.rbprm.rbprm.computeContactPoints(stateId)
return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
......@@ -254,6 +253,51 @@ class FullBody (object):
def interpolateConfigs(self, configs):
return self.client.rbprm.rbprm.interpolateConfigs(configs)
## returns the CWC of the robot at a given state
#
# \param stateId The considered state
# \return H matrix and h column, such that H w <= h
def getContactCone(self, stateId):
H_h = array(self.client.rbprm.rbprm.getContactCone(stateId))
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
## returns the CWC of the robot between two states
#
# \param stateId The first considered state
# \return H matrix and h column, such that H w <= h
def getContactIntermediateCone(self, stateId):
H_h = array(self.client.rbprm.rbprm.getContactIntermediateCone(stateId))
# now decompose cone
return H_h[:,:-1], H_h[:, -1]
## Create a path for the root given
# a set of 3d key points
# The path is composed of n+1 linear interpolations
# between the n positions.
# The rotation is linearly interpolated as well,
# between a start and a goal rotation. The resulting path
# is added to the problem solver
# \param positions array of positions
# \param quat_1 quaternion of 1st rotation
# \param quat_2 quaternion of 2nd rotation
def generateRootPath(self, positions, quat_1, quat_2):
return self.client.rbprm.rbprm.generateRootPath(positions, quat_1, quat_2)
## Create a path for the root given
# a set of 3d key points
# The path is composed of n+1 linear interpolations
# between the n positions.
# The rotation is linearly interpolated as well,
# between a start and a goal configuration. Assuming the robot is a
# free-flyer, rotations are extracted automatically. The resulting path
# is added to the problem solver
# \param positions array of positions
# \param configState1 configuration of 1st rotation
# \param configState2 configuration of 2nd rotation
def generateRootPathStates(self, positions, configState1, configState2):
return self.client.rbprm.rbprm.generateRootPath(positions, configState1[3:7], configState2[3:7])
## Given start and goal states
# Provided a path has already been computed and interpolated, generate a continuous path
# between two indicated states. Will fail if the index of the states do not exist
......
......@@ -4,23 +4,30 @@ import numpy as np
def __get_com(robot, config):
save = robot.getCurrentConfig()
robot.setCurrentConfig(config)
com = robot.getCenterOfMass()
com = robot.getCenterOfMass()
com = config[0:3] #assimilate root and com
robot.setCurrentConfig(save)
return com
def gen_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True):
def gen_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
init_com = __get_com(fullBody, states[state_id])
end_com = __get_com(fullBody, states[state_id+1])
p, N = fullBody.computeContactPoints(state_id)
mass = fullBody.getMass()
t_end_phases = [0]
[t_end_phases.append(t_end_phases[-1]+0.5) for _ in range(len(p))]
print t_end_phases
[t_end_phases.append(t_end_phases[-1]+1) for _ in range(len(p))]
dt = 0.1
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, mu, mass, 9.81, reduce_ineq)
cones = None
if(computeCones):
cones = [fullBody.getContactCone(state_id)[0]]
if(len(p) > 1):
cones.append(fullBody.getContactIntermediateCone(state_id)[0])
if(len(p) > len(cones)):
cones.append(fullBody.getContactCone(state_id+1)[0])
return cone_optimization(p, N, [init_com + [0,0,0], end_com + [0,0,0]], t_end_phases[1:], dt, cones, mu, mass, 9.81, reduce_ineq)
def draw_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True ):
var_final, params = gen_trajectory(fullBody, states, state_id, mu , reduce_ineq)
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, reduce_ineq = True):
var_final, params = gen_trajectory(fullBody, states, state_id, computeCones, mu , reduce_ineq)
p, N = fullBody.computeContactPoints(state_id)
print var_final
import numpy as np
......@@ -49,3 +56,4 @@ def draw_trajectory(fullBody, states, state_id, mu = 1, reduce_ineq = True ):
ax.set_zlabel('Z Label')
plt.show()
return var_final, params
......@@ -27,6 +27,7 @@
#include "hpp/rbprm/stability/stability.hh"
#include "hpp/rbprm/sampling/sample-db.hh"
#include "hpp/model/urdf/util.hh"
#include "hpp/core/straight-path.hh"
#include <fstream>
......@@ -250,15 +251,10 @@ namespace hpp {
return cit->second[sampleId];
}
model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
model::Configuration_t dofArrayToConfig (const std::size_t& deviceDim,
const hpp::floatSeq& dofArray)
{
std::size_t configDim = (std::size_t)dofArray.length();
// Get robot
if (!robot) {
throw hpp::Error ("No robot in problem solver.");
}
std::size_t deviceDim = robot->configSize ();
// Fill dof vector with dof array.
model::Configuration_t config; config.resize (configDim);
for (std::size_t iDof = 0; iDof < configDim; iDof++) {
......@@ -273,18 +269,30 @@ namespace hpp {
return config;
}
std::vector<model::Configuration_t> doubleDofArrayToConfig (const model::DevicePtr_t& robot,
model::Configuration_t dofArrayToConfig (const model::DevicePtr_t& robot,
const hpp::floatSeq& dofArray)
{
return dofArrayToConfig(robot->configSize(), dofArray);
}
T_Configuration doubleDofArrayToConfig (const std::size_t& deviceDim,
const hpp::floatSeqSeq& doubleDofArray)
{
std::size_t configsDim = (std::size_t)doubleDofArray.length();
std::vector<model::Configuration_t> res;
T_Configuration res;
for (_CORBA_ULong iConfig = 0; iConfig < configsDim; iConfig++)
{
res.push_back(dofArrayToConfig(robot, doubleDofArray[iConfig]));
res.push_back(dofArrayToConfig(deviceDim, doubleDofArray[iConfig]));
}
return res;
}
T_Configuration doubleDofArrayToConfig (const model::DevicePtr_t& robot,
const hpp::floatSeqSeq& doubleDofArray)
{
return doubleDofArrayToConfig(robot->configSize(), doubleDofArray);
}
std::vector<std::string> stringConversion(const hpp::Names_t& dofArray)
{
std::vector<std::string> res;
......@@ -577,7 +585,7 @@ namespace hpp {
{
throw std::runtime_error ("End state not initialized, can not interpolate ");
}
std::vector<model::Configuration_t> configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
T_Configuration configurations = doubleDofArrayToConfig(fullBody_->device_, configs);
const affMap_t &affMap = problemSolver_->map
<std::vector<boost::shared_ptr<model::CollisionObject> > > ();
if (affMap.empty ())
......@@ -616,6 +624,142 @@ namespace hpp {
}
}
hpp::floatSeqSeq* contactCone(RbPrmFullBodyPtr_t& fullBody, State& state)
{
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
std::pair<stability::MatrixXX, stability::VectorX> cone = stability::ComputeCentroidalCone(fullBody, state);
res->length ((_CORBA_ULong)cone.first.rows());
_CORBA_ULong size = (_CORBA_ULong) cone.first.cols()+1;
for(int i=0; i < cone.first.rows(); ++i)
{
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
//convert the row in dofseq
for (int j=0 ; j < cone.first.cols() ; ++j)
{
dofArray[j] = cone.first(i,j);
}
dofArray[size-1] =cone.second[i];
(*res) [(_CORBA_ULong)i] = floats;
}
return res;
}
hpp::floatSeqSeq* RbprmBuilder::getContactCone(unsigned short stateId) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size() <= stateId)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
}
return contactCone(fullBody_, lastStatesComputed_[stateId]);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
State intermediary(const State& firstState, const State& thirdState, unsigned short& cId, bool& success)
{
success = false;
std::vector<std::string> breaks;
thirdState.contactBreaks(firstState, breaks);
if(breaks.size() > 1)
{
throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
}
if(breaks.size() == 1)
{
State intermediary(firstState);
intermediary.RemoveContact(*breaks.begin());
success = true;
}
return firstState;
}
hpp::floatSeqSeq* RbprmBuilder::getContactIntermediateCone(unsigned short stateId) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size() <= stateId+1)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId+1)));
}
bool success;
State intermediaryState = intermediary(lastStatesComputed_[stateId], lastStatesComputed_[stateId+1],stateId,success);
if(!success)
{
throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));
}
return contactCone(fullBody_,intermediaryState);
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
core::PathPtr_t makePath(DevicePtr_t device,
const ProblemPtr_t& problem,
model::ConfigurationIn_t q1,
model::ConfigurationIn_t q2)
{
return StraightPath::create(device, q1, q2, (*problem->distance()) (q1, q2));
}
model::Configuration_t addRotation(CIT_Configuration& pit, const model::value_type& u,
model::ConfigurationIn_t q1, model::ConfigurationIn_t q2,
model::ConfigurationIn_t ref)
{
model::Configuration_t res = ref;
res.head(3) = *pit;
res.segment<4>(3) = tools::interpolate(q1, q2, u);
return res;
}
core::PathVectorPtr_t addRotations(const T_Configuration& positions,
model::ConfigurationIn_t q1, model::ConfigurationIn_t q2,
model::ConfigurationIn_t ref, DevicePtr_t device,
const ProblemPtr_t& problem)
{
core::PathVectorPtr_t res = core::PathVector::create(device->configSize(), device->numberDof());
model::value_type size_step = 1 /(model::value_type)(positions.size());
model::value_type u = 0.;
CIT_Configuration pit = positions.begin();
model::Configuration_t previous = addRotation(pit, 0., q1, q2, ref), current;
++pit;
for(;pit != positions.end()-1; ++pit, u+=size_step)
{
current = addRotation(pit, u, q1, q2, ref);
res->appendPath(makePath(device,problem, previous, current));
previous = current;
}
// last configuration is exactly q2
current = addRotation(pit, 1., q1, q2, ref);
res->appendPath(makePath(device,problem, previous, current));
return res;
}
void RbprmBuilder::generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1Seq, const hpp::floatSeq& q2Seq) throw (hpp::Error)
{
try
{
model::Configuration_t q1 = dofArrayToConfig(4, q1Seq), q2 = dofArrayToConfig(4, q2Seq);
T_Configuration positions = doubleDofArrayToConfig(3, rootPositions);
problemSolver_->addPath(addRotations(positions,q1,q2,fullBody_->device_->currentConfiguration(),
fullBody_->device_,problemSolver_->problem()));
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Rotation;
......@@ -661,18 +805,12 @@ namespace hpp {
const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, firstState));
std::vector<std::string> breaks, creations;
thirdState.contactBreaks(firstState, breaks);
if(breaks.size() > 1)
std::vector<std::string> creations;
bool success(false);
State intermediaryState = intermediary(firstState, thirdState, cId, success);
if(success)
{
throw std::runtime_error ("too many contact breaks between states" + std::string(""+cId) +
"and " + std::string(""+(cId + 1)));
}
if(breaks.size() == 1)
{
State intermediary(firstState);
intermediary.RemoveContact(*breaks.begin());
allStates.push_back(computeRectangleContact(fullBody_, intermediary));
allStates.push_back(computeRectangleContact(fullBody_, intermediaryState));
}
thirdState.contactCreations(firstState, creations);
if(creations.size() == 1)
......
......@@ -142,6 +142,10 @@ namespace hpp {
virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactIntermediateCone(unsigned short stateId) throw (hpp::Error);
virtual void generateRootPath(const hpp::floatSeqSeq& rootPositions,
const hpp::floatSeq& q1, const hpp::floatSeq& q2) throw (hpp::Error);
virtual void interpolateBetweenStates(double state1, double state2, unsigned short numOptimizations) throw (hpp::Error);
virtual void interpolateBetweenStatesFromPath(double state1, double state2, unsigned short path, unsigned short numOptimizations) throw (hpp::Error);
virtual void saveComputedStates(const char* filepath) 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