Skip to content
GitLab
Commit 8141c46c authored by stevet's avatar stevet
Browse files

first successful contact plan with hyq [TODO remove frame collision not effector]

parent 84f2daaa
Hide whitespace changes
Inline Side-by-side
script/scenarios/demos/darpa_hyq.py
View file @ 8141c46c
......@@ -29,7 +29,7 @@ srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
# Setting a number of sample configurations used
nbSamples = 20000
......@@ -79,11 +79,11 @@ q_goal = hyq_ref[:]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=hyq_ref[2]+0.02
q_init = [-2.0,
0.0,
0.6638277139631803,
1.0,
0.6838277139631803,
0.0,
0.0,
0.0,
1.0,
0.14279812395541294,
0.934392553166556,
-0.9968239786882757,
......@@ -185,6 +185,19 @@ def contactPlan(step = 0.5):
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
r(configs[i]);
time.sleep(step)
def contactPlanDontMove(step = 0.5):
r.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
a = configs[i]
a[:6] = [0 for _ in range(6)]
a[6] = 1
#~ r(configs[i]);
r(a);
time.sleep(step)
......@@ -208,7 +221,56 @@ def e(step = 0.5):
print "displaying contact plan"
contactPlan(step)
def f(step = 0.5):
print "displaying static contact plan"
contactPlanDontMove(step)
print "Root path generated in " + str(tp.t) + " ms."
#~ d();e()
d(0.004);e(0.01)
d(0.1);e(0.01)
#~ d(0.004);e(0.01)
from hpp.corbaserver.rbprm.rbprmstate import *
com = fullBody.client.basic.robot.getCenterOfMass
s = None
def d1():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d2():
global s
s = State(fullBody,q = q_init, limbsIncontact = [larmId, rarmId])
a = s.q()
a[2]=a[2]+0.05
a[0]=a[0]+0.05
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d3():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]+0.01
s.setQ(a)
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
def d4():
global s
s = State(fullBody,q = q_init, limbsIncontact = [rarmId])
a = s.q()
a[2]=a[2]-0.01
s.setQ(a)
print s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
s = State(fullBody,q = s.q(), limbsIncontact = [larmId])
return s.projectToCOM([0.01,0.,0.], maxNumSample = 0)
script/scenarios/demos/darpa_hyq_path.py
View file @ 8141c46c
......@@ -17,7 +17,7 @@ srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
rbprmBuilder.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
......@@ -26,7 +26,7 @@ rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
# We also bound the rotations of the torso.
rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
#~ rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
......@@ -37,7 +37,8 @@ r = Viewer (ps)
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
#~ q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
q_goal [0:3] = [-1, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
......@@ -46,19 +47,23 @@ ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
#~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel (packageName, "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConFigurationShooter("RbprmShooter")
ps.client.problem.selectConfigurationShooter("RbprmShooter")
ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Solve the problem
t = ps.solve ()
#~ t = 0.
if isinstance(t, list):
t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
print "computation time for root path ", t
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (rbprmBuilder.client.basic, r)
......
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 8141c46c
......@@ -941,6 +941,8 @@ class FullBody (object):
def getLinkName (self, jointName):
return self.client.basic.robot.getLinkName (jointName)
## \}
def getLinkNames (self, jointName):
return self.client.basic.robot.getLinkNames (jointName)
## \name Access to current configuration
#\{
......
src/rbprmbuilder.impl.cc
View file @ 8141c46c
......@@ -62,6 +62,9 @@ namespace hpp {
typedef spline::bezier_curve<> bezier;
namespace impl {
const pinocchio::Device::Computation_t flag = static_cast <pinocchio::Device::Computation_t>
(pinocchio::Device::JOINT_POSITION | pinocchio::Device::JACOBIAN | pinocchio::Device::COM);
RbprmBuilder::RbprmBuilder ()
: POA_hpp::corbaserver::rbprm::RbprmBuilder()
, romLoaded_(false)
......@@ -135,7 +138,7 @@ namespace hpp {
// Add device to the planner
problemSolver()->robot (device);
problemSolver()->robot ()->controlComputation
(pinocchio::Device::JOINT_POSITION);
(flag);
}
catch (const std::exception& exc)
{
......@@ -184,7 +187,7 @@ namespace hpp {
problemSolver()->robot (fullBody()->device_);
problemSolver()->resetProblem();
problemSolver()->robot ()->controlComputation
(pinocchio::Device::JOINT_POSITION);
(flag);
refPose_ = fullBody()->device_->currentConfiguration();
}
catch (const std::exception& exc)
......@@ -205,7 +208,7 @@ namespace hpp {
problemSolver()->robot (fullBody()->device_);
problemSolver()->resetProblem();
problemSolver()->robot ()->controlComputation
(pinocchio::Device::JOINT_POSITION);
(flag);
}
catch (const std::exception& exc)
{
......@@ -295,9 +298,9 @@ namespace hpp {
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().rotation() * limb->normal_;
const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
const fcl::Matrix3f rotation = alignRotation * limb->effector_->currentTransformation().rotation();
const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
const fcl::Vec3f offset = rotation * limb->offset_;
const double& lx = limb->x_, ly = limb->y_;
p << lx, ly, 0,
......@@ -371,9 +374,9 @@ namespace hpp {
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().rotation() * limb->normal_;
const fcl::Vec3f z = limb->effector_.currentTransformation().rotation() * limb->normal_;
const fcl::Matrix3f alignRotation = tools::GetRotationMatrix(z,normal);
const fcl::Matrix3f rotation = alignRotation * limb->effector_->currentTransformation().rotation();
const fcl::Matrix3f rotation = alignRotation * limb->effector_.currentTransformation().rotation();
const fcl::Vec3f offset = rotation * limb->offset_;
const double& lx = limb->x_, ly = limb->y_;
p << lx, ly, 0,
......@@ -490,10 +493,10 @@ namespace hpp {
State state;
state.configuration_ = config;
state.contacts_[limbName] = true;
const fcl::Vec3f position = limb->effector_->currentTransformation().translation();
const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
state.contactPositions_[limbName] = position;
state.contactNormals_[limbName] = fcl::Vec3f(0,0,1);
state.contactRotation_[limbName] = limb->effector_->currentTransformation().rotation();
state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
std::vector<fcl::Vec3f> poss = (computeRectangleContact(fullBody(), state));
hpp::floatSeqSeq *res;
......@@ -807,10 +810,10 @@ namespace hpp {
rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
const std::string& limbName = *cit;
state.contacts_[limbName] = true;
const fcl::Vec3f position = limb->effector_->currentTransformation().translation();
const fcl::Vec3f position = limb->effector_.currentTransformation().translation();
state.contactPositions_[limbName] = position;
state.contactNormals_[limbName] = limb->effector_->currentTransformation().rotation() * limb->normal_;
state.contactRotation_[limbName] = limb->effector_->currentTransformation().rotation();
state.contactNormals_[limbName] = limb->effector_.currentTransformation().rotation() * limb->normal_;
state.contactRotation_[limbName] = limb->effector_.currentTransformation().rotation();
state.contactOrder_.push(limbName);
}
state.nbContacts = state.contactNormals_.size();
......@@ -902,15 +905,17 @@ namespace hpp {
for(std::vector<std::string>::const_iterator cit = names.begin(); cit !=names.end(); ++cit)
{
rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
pinocchio::Transform3f localFrame, globalFrame;
pinocchio::Transform3f localFrame(1), globalFrame(1);
localFrame.translation(-limb->offset_);
std::vector<bool> posConstraints;
std::vector<bool> rotConstraints;
posConstraints.push_back(false);posConstraints.push_back(false);posConstraints.push_back(true);
rotConstraints.push_back(true);rotConstraints.push_back(true);rotConstraints.push_back(true);
pinocchio::Frame effectorFrame = device->getFrameByName(limb->effector_.name());
pinocchio::JointPtr_t effectorJoint(new pinocchio::Joint(limb->effector_.joint()));
proj->add(core::NumericalConstraint::create (constraints::Position::create("",fullBody()->device_,
limb->effector_,
globalFrame,
effectorJoint,
effectorFrame.positionInParentFrame() * globalFrame,
localFrame,
posConstraints)));
if(limb->contactType_ == hpp::rbprm::_6_DOF)
......@@ -919,9 +924,9 @@ namespace hpp {
value_type theta = 2*(value_type(rand()) / value_type(RAND_MAX) - 0.5) * M_PI;
fcl::Matrix3f r = tools::GetZRotMatrix(theta);
// TODO not assume identity matrix for effector frame
fcl::Matrix3f rotation = r;// * limb->effector_->initialPosition ().getRotation();
fcl::Matrix3f rotation = effectorFrame.currentTransformation().rotation() * r;// * limb->effector_->initialPosition ().getRotation();
proj->add(core::NumericalConstraint::create (constraints::Orientation::create("",fullBody()->device_,
limb->effector_,
effectorJoint,
pinocchio::Transform3f(rotation,fcl::Vec3f::Zero()),
rotConstraints)));
}
......@@ -938,8 +943,8 @@ namespace hpp {
std::string limbId = *cit;
rbprm::RbPrmLimbPtr_t limb = fullBody()->GetLimbs().at(*cit);
tmp.contacts_[limbId] = true;
tmp.contactPositions_[limbId] = limb->effector_->currentTransformation().translation();
tmp.contactRotation_[limbId] = limb->effector_->currentTransformation().rotation();
tmp.contactPositions_[limbId] = limb->effector_.currentTransformation().translation();
tmp.contactRotation_[limbId] = limb->effector_.currentTransformation().rotation();
tmp.contactNormals_[limbId] = z;
tmp.configuration_ = config;
++tmp.nbContacts;
......@@ -1232,8 +1237,8 @@ namespace hpp {
throw std::runtime_error ("Impossible to find limb for joint "
+ (*cit) + " to robot; limb not defined");
}
const core::JointPtr_t joint = fullBody->device_->getJointByName(lit->second->effector_->name());
const pinocchio::Transform3f& transform = joint->currentTransformation ();
const pinocchio::Frame frame = fullBody->device_->getFrameByName(lit->second->effector_.name());
const pinocchio::Transform3f& transform = frame.currentTransformation ();
const fcl::Matrix3f& rot = transform.rotation();
state.contactPositions_[*cit] = transform.translation();
state.contactRotation_[*cit] = rot;
......@@ -2617,7 +2622,7 @@ assert(s2 == s1 +1);
}
}
std::string effectorVar = s2.contactCreations(s1).front();
JointPtr_t effector = fullBody()->device_->getJointByName(fullBody()->GetLimbs().at(effectorVar)->effector_->name());
pinocchio::Frame effector = fullBody()->device_->getFrameByName(fullBody()->GetLimbs().at(effectorVar)->effector_.name());
std::vector<PathVectorPtr_t> listBeziers = interpolation::fitBeziersToPath(fullBody(),effector,paths[comTraj]->length(),fullBodyComPath,s1,s2);
hpp::floatSeqSeq *res;
......@@ -3020,10 +3025,10 @@ assert(s2 == s1 +1);
{
const hpp::rbprm::RbPrmLimbPtr_t limb =fullBody()->GetLimbs().at(std::string(*cit));
testedState.contacts_[*cit] = true;
testedState.contactPositions_[*cit] = limb->effector_->currentTransformation().translation();
testedState.contactRotation_ [*cit] = limb->effector_->currentTransformation().rotation();
testedState.contactPositions_[*cit] = limb->effector_.currentTransformation().translation();
testedState.contactRotation_ [*cit] = limb->effector_.currentTransformation().rotation();
// normal given by effector normal
const fcl::Vec3f normal = limb->effector_->currentTransformation().rotation() * limb->normal_;
const fcl::Vec3f normal = limb->effector_.currentTransformation().rotation() * limb->normal_;
testedState.contactNormals_[*cit] = normal;
testedState.configuration_ = config;
++testedState.nbContacts;
......@@ -3329,7 +3334,7 @@ assert(s2 == s1 +1);
if(lit->second->kinematicConstraints_.first.size()==0){
hppDout(notice,"Kinematics constraints not initialized");
}else{
successLimb = rbprm::reachability::verifyKinematicConstraints(lit->second->kinematicConstraints_,lit->second->effector_->currentTransformation(),pt);
successLimb = rbprm::reachability::verifyKinematicConstraints(lit->second->kinematicConstraints_,lit->second->effector_.currentTransformation(),pt);
hppDout(notice,"kinematic constraints verified : "<<successLimb);
success = success && successLimb;
}
......
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