Skip to content
Snippets Groups Projects
Commit 4173e5d4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

contact accurately created along surface

parent 53d41fe8
No related branches found
No related tags found
No related merge requests found
......@@ -94,7 +94,7 @@ module hpp
floatSeq generateContacts(in floatSeq dofArray, in floatSeq direction) raises (Error);
floatSeq getContactSamplesIds(in string name, in floatSeq dofArray, in floatSeq direction) raises (Error);
void addLimb(in string limb, in unsigned short samples, in double resolution) raises (Error);
void addLimb(in string limb, in floatSeq offset, in unsigned short samples, in double resolution) raises (Error);
}; // interface Robot
}; // module rbprm
......
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
packageName = "hrp2_14_description"
meshPackageName = "hrp2_14_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "hrp2_14"
urdfSuffix = ""
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-1, 2, -2, 0, -1, 1.5])
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( fullBody )
r = Viewer (ps)
r.loadObstacleModel ('hpp-rbprm-corba', "scene", "car")
#~
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
fullBody.addLimb(rLeg,rLegOffset, 5000, 0.001)
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
fullBody.addLimb(lLeg,lLegOffset, 5000, 0.001)
q_init = fullBody.getCurrentConfig ()
q_init [0:3] = [0, -0.5, 0.7]
r (q_init)
fullBody.setCurrentConfig (q_init)
#~
#~
q_init = fullBody.generateContacts(q_init, [0,0,1])
r (q_init)
#~
fullBody.getContactSamplesIds(rLeg, q_init, [0,0,1])
#~ q_init = fullBody.getSample(rLeg, 1)
#~ r (q_init)
ids = fullBody.getContactSamplesIds(rLeg, q_init, [0,0,1])
i =-1
#~ i=i+1; q_init = fullBody.getSample(rLeg, int(ids[i])); r(q_init); ids[i]
......@@ -71,8 +71,8 @@ class FullBody (object):
self.rankInVelocity [j] = rankInVelocity
rankInVelocity += self.client.basic.robot.getJointNumberDof (j)
def addLimb(self, name, samples, resolution):
self.client.rbprm.rbprm.addLimb(name, samples, resolution)
def addLimb(self, name, offset, samples, resolution):
self.client.rbprm.rbprm.addLimb(name, offset, samples, resolution)
def getSample(self, name, idsample):
return self.client.rbprm.rbprm.getSampleConfig(name,idsample)
......
......@@ -187,14 +187,16 @@ namespace hpp {
throw Error ("No full body robot was loaded");
try
{
Eigen::Vector3d dir;
fcl::Vec3f dir;
for(std::size_t i =0; i <3; ++i)
{
dir[i] = direction[i];
}
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
std::cout << "configuration in " << config << std::endl;
rbprm::State state = rbprm::ComputeContacts(fullBody_,config,
problemSolver_->collisionObstacles(), dir);
std::cout << "configuration out" << state.configuration_ << std::endl;
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(_CORBA_ULong(config.rows()));
for(std::size_t i=0; i< _CORBA_ULong(config.rows()); i++)
......@@ -213,7 +215,7 @@ namespace hpp {
throw Error ("No full body robot was loaded");
try
{
Eigen::Vector3d dir;
fcl::Vec3f dir;
for(std::size_t i =0; i <3; ++i)
{
dir[i] = direction[i];
......@@ -255,13 +257,18 @@ namespace hpp {
}
}
void RbprmBuilder::addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error)
void RbprmBuilder::addLimb(const char* limb, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
try
{
fullBody_->AddLimb(std::string(limb),samples,resolution);
fcl::Vec3f off;
for(std::size_t i =0; i <3; ++i)
{
off[i] = offset[i];
}
fullBody_->AddLimb(std::string(limb), off, problemSolver_->collisionObstacles(), samples,resolution);
}
catch(std::runtime_error& e)
{
......
......@@ -84,7 +84,7 @@ namespace hpp {
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
virtual void addLimb(const char* limb, unsigned short samples, double resolution) throw (hpp::Error);
virtual void addLimb(const char* limb, const hpp::floatSeq& offset, unsigned short samples, double resolution) throw (hpp::Error);
public:
void SetProblemSolver (hpp::core::ProblemSolverPtr_t problemSolver);
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment