Commit 1ccaa86d authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added isLimbInContact method

parent f359314e
......@@ -317,6 +317,17 @@ module hpp
/// \return projected configuration
floatSeq projectToCom(in double state, in floatSeq targetCom) raises (Error);
/// \param limb name of the limb for which the request aplies
/// \param state1 current state considered
/// \return whether the limb is in contact at this state
short isLimbInContact(in string limbname, in double state1) raises (Error);
/// Is limb in contact during the motion from the current state to the next one
/// \param limb name of the limb for which the request aplies
/// \param state1 current state considered
/// \return whether the limb is in contact at this state
short isLimbInContactIntermediary(in string limbname, in double state1) raises (Error);
/// Saves the last computed states by the function interpolate in a filename.
/// Raises an error if interpolate has not been called, or the file could not be opened.
/// \param filename name of the file used to save the contacts.
......
......@@ -35,16 +35,24 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
global constraintsLoaded
As = []; bs = []
fullBody.setCurrentConfig(config)
contacts = []
for i, v in limbsCOMConstraints.iteritems():
if not constraintsComLoaded.has_key(i):
constraintsComLoaded[i] = ineq_from_file(ineqPath+v['file'])
print "inter", interm
print "intermed", fullBody.isLimbInContactIntermediary(i, state)
print "inter", fullBody.isLimbInContact(i, state)
contact = (interm and fullBody.isLimbInContactIntermediary(i, state)) or (not interm and fullBody.isLimbInContact(i, state))
if contact:
ineq = constraintsComLoaded[i]
qEffector = fullBody.getJointPosition(v['effector'])
tr = quaternion_matrix(qEffector[3:7])
tr = quaternion_matrix(qEffector[3:7])
tr[:3,3] = np.array(qEffector[0:3])
ineq_r = rotate_inequalities(ineq, tr)
As.append(ineq_r.A); bs.append(ineq_r.b);
print 'contact', v['effector']
contacts.append(v['effector'])
print 'contacts', contacts
return [np.vstack(As), np.hstack(bs)]
......
......@@ -693,6 +693,12 @@ namespace hpp {
}
bool success;
State intermediaryState = intermediary(lastStatesComputed_[stateId], lastStatesComputed_[stateId+1],stateId,success);
for(std::map<std::string, fcl::Vec3f>::const_iterator cit = intermediaryState.contactPositions_.begin();
cit != intermediaryState.contactPositions_.end(); ++cit)
{
std::cout << cit->first << std::endl;
std::cout << "WTF " << std::endl;
}
if(!success)
{
throw std::runtime_error ("No contact breaks, hence no intermediate state from state " + std::string(""+(stateId)));
......
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