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

can project state to new contact location

parent f177a0de
......@@ -554,6 +554,17 @@ module hpp
/// \param logFile name of the file where to dump the profiling data
void dumpProfile(in string logFile) raises (Error);
/// tries to add a new contact to the state
/// if the limb is already in contact, replace the
/// previous contact. Only considers kinematic limitations.
/// collision and equilibrium are NOT considered.
/// \param state State considered
/// \param limbName name of the considered limb to create contact with
/// \param p 3d position of the point
/// \param n 3d normal of the contact location center
/// \return (success,NState) whether the creation was successful, as well as the new state
short addNewContact(in unsigned short stateId, in string limbName, in floatSeq position, in floatSeq normal) raises (Error);
}; // interface Robot
}; // module rbprm
}; // module corbaserver
......
......@@ -102,6 +102,7 @@ INSTALL(
${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/state_alg.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/corbaserver/rbprm/problem_solver.py
DESTINATION ${PYTHON_SITELIB}/hpp/corbaserver/rbprm
)
......
......@@ -92,11 +92,11 @@ class State (object):
# \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)
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)
rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,1, limbName)
else:
rawdata = cl.computeContactPointsAtStateForLimb(self.sId,0)
rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,0, limbName)
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]
......
#!/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.rbprmstate import State
## algorithmic methods on state
## tries to add a new contact to the state
## if the limb is already in contact, replace the
## previous contact. Only considers kinematic limitations.
## collision and equilibrium are NOT considered.
# \param state State considered
# \param limbName name of the considered limb to create contact with
# \param p 3d position of the point
# \param n 3d normal of the contact location center
# \return (success,NState) whether the creation was successful, as well as the new state
def addNewContact(state, limbName, p, n):
return State(state.fullBody, state.cl.addNewContact(state.sId, limbName, p, n))
......@@ -240,7 +240,7 @@ namespace hpp {
typedef Eigen::Ref<Matrix43> Ref_matrix43;
std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
const rbprm::State& state)
const rbprm::State& state, const std::vector<std::string> limbSelected = std::vector<std::string>())
{
device->device_->currentConfiguration(state.configuration_);
device->device_->computeForwardKinematics();
......@@ -252,66 +252,69 @@ namespace hpp {
cit != state.contactPositions_.end(); ++cit)
{
const std::string& name = cit->first;
const fcl::Vec3f& position = cit->second;
limb = limbs.at(name);
const fcl::Vec3f& normal = state.contactNormals_.at(name);
const fcl::Vec3f z = limb->effector_->currentTransformation().getRotation() * limb->normal_;
const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
const fcl::Matrix3f rotation = alignRotation * limb->effector_->currentTransformation().getRotation();
const fcl::Vec3f offset = rotation * limb->offset_;
const double& lx = limb->x_, ly = limb->y_;
p << lx, ly, 0,
lx, -ly, 0,
-lx, -ly, 0,
-lx, ly, 0;
if(limb->contactType_ == _3_DOF)
{
//create rotation matrix from normal
fcl::Vec3f z_fcl = state.contactNormals_.at(name);
Eigen::Vector3d z,x,y;
for(int i =0; i<3; ++i) z[i] = z_fcl[i];
x = z.cross(Eigen::Vector3d(0,-1,0));
if(x.norm() < 10e-6)
if(limbSelected.empty() || std::find(limbSelected.begin(), limbSelected.end(), name) != limbSelected.end())
{
const fcl::Vec3f& position = cit->second;
limb = limbs.at(name);
const fcl::Vec3f& normal = state.contactNormals_.at(name);
const fcl::Vec3f z = limb->effector_->currentTransformation().getRotation() * limb->normal_;
const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
const fcl::Matrix3f rotation = alignRotation * limb->effector_->currentTransformation().getRotation();
const fcl::Vec3f offset = rotation * limb->offset_;
const double& lx = limb->x_, ly = limb->y_;
p << lx, ly, 0,
lx, -ly, 0,
-lx, -ly, 0,
-lx, ly, 0;
if(limb->contactType_ == _3_DOF)
{
y = z.cross(fcl::Vec3f(1,0,0));
y.normalize();
x = y.cross(z);
//create rotation matrix from normal
fcl::Vec3f z_fcl = state.contactNormals_.at(name);
Eigen::Vector3d z,x,y;
for(int i =0; i<3; ++i) z[i] = z_fcl[i];
x = z.cross(Eigen::Vector3d(0,-1,0));
if(x.norm() < 10e-6)
{
y = z.cross(fcl::Vec3f(1,0,0));
y.normalize();
x = y.cross(z);
}
else
{
x.normalize();
y = z.cross(x);
}
R.block<3,1>(0,0) = x;
R.block<3,1>(0,1) = y;
R.block<3,1>(0,2) = z;
/*for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*(p.row(i).transpose())) + offset);
res.push_back(state.contactNormals_.at(name));
}*/
res.push_back(position + (R*(offset)));
res.push_back(state.contactNormals_.at(name));
}
else
{
x.normalize();
y = z.cross(x);
}
R.block<3,1>(0,0) = x;
R.block<3,1>(0,1) = y;
R.block<3,1>(0,2) = z;
/*for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*(p.row(i).transpose())) + offset);
res.push_back(state.contactNormals_.at(name));
}*/
res.push_back(position + (R*(offset)));
res.push_back(state.contactNormals_.at(name));
}
else
{
const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
for(int i =0; i< 3; ++i)
for(int j =0; j<3;++j)
R(i,j) = fclRotation(i,j);
fcl::Vec3f z_axis(0,0,1);
fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*(rotationLocal*(p.row(i).transpose() + limb->offset_))));
res.push_back(state.contactNormals_.at(name));
const fcl::Matrix3f& fclRotation = state.contactRotation_.at(name);
for(int i =0; i< 3; ++i)
for(int j =0; j<3;++j)
R(i,j) = fclRotation(i,j);
fcl::Vec3f z_axis(0,0,1);
fcl::Matrix3f rotationLocal = tools::GetRotationMatrix(z_axis, limb->normal_);
for(std::size_t i =0; i<4; ++i)
{
res.push_back(position + (R*(rotationLocal*(p.row(i).transpose() + limb->offset_))));
res.push_back(state.contactNormals_.at(name));
}
}
}
}
return res;
}
std::vector<fcl::Vec3f> computeRectangleContact(const rbprm::RbPrmFullBodyPtr_t device,
std::vector<fcl::Vec3f> computeRectangleContactLocalTr(const rbprm::RbPrmFullBodyPtr_t device,
const rbprm::State& state,
const std::string& limbName)
{
......@@ -1029,7 +1032,6 @@ namespace hpp {
state.nbContacts = state.contactNormals_.size() ;
state.configuration_ = config;
state.robustness = stability::IsStable(fullBody,state);
std::cout << "is stable " << state.robustness << std::endl;
state.stable = state.robustness >= 0;
fullBody->device_->currentConfiguration(old);
}
......@@ -1057,7 +1059,7 @@ namespace hpp {
std::vector<std::string> limbs; limbs.push_back(limbNam);
SetPositionAndNormal(state,fullBody_, configuration, limbs);
const std::vector<fcl::Vec3f>& positions = computeRectangleContact(fullBody_,state,limb);
const std::vector<fcl::Vec3f>& positions = computeRectangleContactLocalTr(fullBody_,state,limb);
_CORBA_ULong size = (_CORBA_ULong) (positions.size () * 3);
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(size);
......@@ -1529,18 +1531,18 @@ namespace hpp {
std::string limb(limbName);
const State& firstState = lastStatesComputed_[cId], thirdState = lastStatesComputed_[cId+1];
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, firstState, limb));
allStates.push_back(computeRectangleContactLocalTr(fullBody_, firstState, limb));
std::vector<std::string> creations;
bool success(false);
State intermediaryState = intermediary(firstState, thirdState, cId, success);
if(success)
{
allStates.push_back(computeRectangleContact(fullBody_, intermediaryState, limb));
allStates.push_back(computeRectangleContactLocalTr(fullBody_, intermediaryState, limb));
}
thirdState.contactCreations(firstState, creations);
if(creations.size() == 1)
{
allStates.push_back(computeRectangleContact(fullBody_, thirdState, limb));
allStates.push_back(computeRectangleContactLocalTr(fullBody_, thirdState, limb));
}
if(creations.size() > 1)
{
......@@ -1592,7 +1594,8 @@ namespace hpp {
state = interm;
}
std::vector<std::vector<fcl::Vec3f> > allStates;
allStates.push_back(computeRectangleContact(fullBody_, state,limb));
std::vector<std::string> limbs ; limbs.push_back(limb);
allStates.push_back(computeRectangleContact(fullBody_, state,limbs));
hpp::floatSeqSeq *res;
res = new hpp::floatSeqSeq ();
......@@ -2368,6 +2371,36 @@ assert(s2 == s1 +1);
}
}
CORBA::Short RbprmBuilder::addNewContact(unsigned short stateId, const char* limbName,
const hpp::floatSeq& position, const hpp::floatSeq& normal) throw (hpp::Error)
{
try
{
if(lastStatesComputed_.size() <= stateId)
throw std::runtime_error ("Unexisting state " + std::string(""+(stateId)));
const State& ns = lastStatesComputed_[stateId];
const std::string limb(limbName);
model::Configuration_t config = dofArrayToConfig (std::size_t(3), position);
fcl::Vec3f p; for(int i =0; i<3; ++i) p[i] = config[i];
config = dofArrayToConfig (std::size_t(3), normal);
fcl::Vec3f n; for(int i =0; i<3; ++i) n[i] = config[i];
projection::ProjectionReport rep = projection::projectStateToObstacle(fullBody_,limb, fullBody_->GetLimbs().at(limb), ns, n,p);
if(rep.success_)
{
lastStatesComputed_.push_back(rep.result_);
return lastStatesComputed_.size() -1;
}
return -1;
}
catch(std::runtime_error& e)
{
std::cout << "ERROR " << e.what() << std::endl;
throw Error(e.what());
}
}
void RbprmBuilder::dumpProfile(const char* logFile) throw (hpp::Error)
{
try
......
......@@ -219,6 +219,8 @@ namespace hpp {
virtual void runSampleAnalysis(const char* analysis, double isstatic) throw (hpp::Error);
virtual hpp::floatSeq* runLimbSampleAnalysis(const char* limbname, const char* analysis, double isstatic) throw (hpp::Error);
virtual void dumpProfile(const char* logFile) throw (hpp::Error);
virtual CORBA::Short addNewContact(unsigned short stateId, const char* limbName,
const hpp::floatSeq& position, const hpp::floatSeq& normal) throw (hpp::Error);
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
......
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