Commit f177a0de authored by t steve's avatar t steve
Browse files

added state helper class (finally)

parent b2f3fea7
......@@ -275,6 +275,19 @@ module hpp
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtStateForLimb(in unsigned short stateId, in unsigned short isIntermediate, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state
/// \param stateId iD of the considered state
/// \param isIntermediate whether the intermediate state should be considerred rather than this one
/// \return list of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsAtState(in unsigned short stateId, in unsigned short isIntermediate) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
/// to this function. If these conditions are not met an error is raised.
......
......@@ -101,6 +101,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/client.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmbuilder.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmfullbody.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/rbprmstate.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
)
......
This diff is collapsed.
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Steve Tonneau
#
# This file is part of hpp-rbprm-corba.
# hpp-rbprm-corba 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-corba 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-corba. If not, see
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
from numpy import array
## Creates a state given an Id pointing to an existing c++ state
#
# A RbprmDevice robot is a set of two robots. One for the
# trunk of the robot, one for the range of motion
class State (object):
## Constructor
def __init__ (self, fullBody, sId, isIntermediate = False):
self.cl = fullBody.client.rbprm.rbprm
self.sId = sId
self.isIntermediate = isIntermediate
self.fullBody = fullBody
## assert for case where functions can't be used with intermediate state
def _cni(self):
assert not self.isIntermediate, "method can't be called with intermediate state"
## Get the state configuration
def q(self):
self._cni()
return self.cl.getConfigAtState(self.sId)
## Set the state configuration
# \param q configuration of the robot
# \return whether or not the configuration was successfully set
def setQ(self, q):
self._cni()
return self.cl.setConfigAtState(self.sId, q) > 0
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return whether the limb is in contact at this state
def isLimbInContact(self, limbname):
if(self.isIntermediate):
return self.cl.isLimbInContactIntermediary(limbname, self.sId) >0
else:
return self.cl.isLimbInContact(limbname, self.sId) >0
#
# \param state1 current state considered
# \param limb name of the limb for which the request aplies
# \return all limbs in contact at this state
def getLimbsInContact(self):
return [limbName for limbName in self.fullBody.limbNames if self.isLimbInContact(limbName)]
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def getEffectorPosition(self, limbName):
self._cni()
return self.cl.getEffectorPosition(limbName,self.q())
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def getContactPosAndNormals(self):
if(self.isIntermediate):
rawdata = self.cl.computeContactPointsAtState(self.sId, 1)
else:
rawdata = self.cl.computeContactPointsAtState(self.sId, 0)
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]
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def getContactPosAndNormalsForLimb(self, limbName):
assert self.isLimbInContact(limbname), "in getContactPosAndNormals: limb " + limbName + "is not in contact at state" + str(stateId)
if(self.isIntermediate):
rawdata = cl.computeContactPointsAtStateForLimb(self.sId,1)
else:
rawdata = cl.computeContactPointsAtStateForLimb(self.sId,0)
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]
## Get position of center of mass
def getCenterOfMass (self):
q0 = fullBody.client.basic.robot.getCurrentConfig()
fullBody.client.basic.robot.setCurrentConfig(self.q())
c = fullBody.client.basic.robot.getComPosition()
fullBody.client.basic.robot.setCurrentConfig(q0)
return c
## Get the end effector position for a given configuration, assuming z normal
# \param limbName name of the limb from which to retrieve a sample
# \param configuration configuration of the robot
# \return world position of the limb end effector given the current robot configuration.
# array of size 4, where each entry is the position of a corner of the contact surface
def getContactCone(self, friction):
if(self.isIntermediate):
rawdata = self.cl.getContactIntermediateCone(self.sId,friction)
else:
rawdata = self.cl.getContactCone(self.sId,friction)
H_h = array(rawdata)
return H_h[:,:-1], H_h[:, -1]
......@@ -1473,6 +1473,53 @@ namespace hpp {
return res;
}
floatSeqSeq* RbprmBuilder::computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error)
{
if(lastStatesComputed_.size() <= cId + isIntermediate)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(cId)));
}
State state = lastStatesComputed_[cId];
if(isIntermediate > 0)
{
const State& thirdState = lastStatesComputed_[cId+1];
bool success(false);
State interm = intermediary(state, thirdState, cId, success);
if(success)
state = interm;
}
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, state));
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
// compute array of contact positions
res->length ((_CORBA_ULong)allStates.size());
std::size_t i=0;
for(std::vector<std::vector<fcl::Vec3f> >::const_iterator cit = allStates.begin();
cit != allStates.end(); ++cit, ++i)
{
const std::vector<fcl::Vec3f>& positions = *cit;
_CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
//convert the config in dofseq
for(std::size_t h = 0; h<positions.size(); ++h)
{
for(std::size_t k =0; k<3; ++k)
{
model::size_type j (h*3 + k);
dofArray[j] = positions[h][k];
}
}
(*res) [(_CORBA_ULong)i] = floats;
}
return res;
}
floatSeqSeq* RbprmBuilder::computeContactPointsForLimb(unsigned short cId, const char *limbName) throw (hpp::Error)
{
if(lastStatesComputed_.size() <= cId + 1)
......@@ -1528,6 +1575,53 @@ namespace hpp {
return res;
}
floatSeqSeq* RbprmBuilder::computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char *limbName) throw (hpp::Error)
{
if(lastStatesComputed_.size() <= cId + isIntermediate)
{
throw std::runtime_error ("Unexisting state " + std::string(""+(cId)));
}
std::string limb(limbName);
State state = lastStatesComputed_[cId];
if(isIntermediate > 0)
{
const State& thirdState = lastStatesComputed_[cId+1];
bool success(false);
State interm = intermediary(state, thirdState, cId, success);
if(success)
state = interm;
}
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, state,limb));
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
// compute array of contact positions
res->length ((_CORBA_ULong)allStates.size());
std::size_t i=0;
for(std::vector<std::vector<fcl::Vec3f> >::const_iterator cit = allStates.begin();
cit != allStates.end(); ++cit, ++i)
{
const std::vector<fcl::Vec3f>& positions = *cit;
_CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
double* dofArray = hpp::floatSeq::allocbuf(size);
hpp::floatSeq floats (size, size, dofArray, true);
//convert the config in dofseq
for(std::size_t h = 0; h<positions.size(); ++h)
{
for(std::size_t k =0; k<3; ++k)
{
model::size_type j (h*3 + k);
dofArray[j] = positions[h][k];
}
}
(*res) [(_CORBA_ULong)i] = floats;
}
return res;
}
floatSeqSeq* RbprmBuilder::interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error)
{
try
......
......@@ -154,7 +154,9 @@ namespace hpp {
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsAtState(unsigned short cId, unsigned short isIntermediate) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsAtStateForLimb(unsigned short cId, unsigned short isIntermediate, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolateConfigs(const hpp::floatSeqSeq& configs, double robustnessTreshold, unsigned short filterStates) throw (hpp::Error);
virtual hpp::floatSeqSeq* getContactCone(unsigned short stateId, double friction) 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