Skip to content
GitLab
Commit 790286d4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

unfinished projection

parent c5fc6077
Hide whitespace changes
Inline Side-by-side
src/hpp/corbaserver/rbprm/rbprmfullbody.py
View file @ 790286d4
......@@ -253,6 +253,14 @@ class FullBody (object):
# \param contacts the array of limbs in contact
def setStartState(self, configuration, contacts):
return self.client.rbprm.rbprm.setStartState(configuration, contacts)
## Create a state given a configuration and contacts
#
# \param configuration the desired start configuration
# \param contacts the array of limbs in contact
# \return id of created state
def createState(self, configuration, contacts):
return self.client.rbprm.rbprm.createState(configuration, contacts)
## Initialize the last configuration of the path discretization
# with a balanced configuration for the interpolation problem;
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory.py
View file @ 790286d4
......@@ -159,6 +159,7 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, dt=0.2, phase_dt = [0.4, 1], reduce_ineq = True, verbose = False, limbsCOMConstraints = None, use_window = 0):
var_final, params, elapsed = gen_trajectory(fullBody, states, state_id, computeCones, mu , dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, False, use_window = use_window)
p, N = fullBody.computeContactPoints(state_id)
print "p", p
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
n = 100
......@@ -180,8 +181,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
#~ plt.show()
plt.savefig('/tmp/c'+ str(state_id)+ '.png')
plt.show()
#~ plt.savefig('/tmp/c'+ str(state_id)+ '.png')
print "plotting speed "
print "end target ", params['x_end']
......@@ -199,8 +200,8 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
#~ plt.show()
plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
plt.show()
#~ plt.savefig('/tmp/dc'+ str(state_id)+ '.png')
print "plotting acceleration "
fig = plt.figure()
......@@ -215,7 +216,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
#~ plt.show()
plt.show()
plt.savefig('/tmp/ddc'+ str(state_id)+ '.png')
print "plotting Dl "
......@@ -227,7 +228,7 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
ax.scatter(xs, ys, c='b')
#~ plt.show()
plt.show()
plt.savefig('/tmp/dL'+ str(state_id)+ '.png')
return var_final, params, elapsed
......
src/hpp/corbaserver/rbprm/tools/cwc_trajectory_helper.py
View file @ 790286d4
......@@ -158,16 +158,16 @@ trackedEffectors = []):
global trajec
global trajec_mil
frame_rate = 1./24.
frame_rate_andrea = 1./1000.
#~ frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
#~ frame_rate = 1./25.
#~ frame_rate_andrea = 1./1001.
new_traj = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path, frame_rate)
new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_traj_andrea = gen_trajectory_to_play(fullBody, pp, trajectory, time_per_path,frame_rate_andrea)
#~ new_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
com = genComPerFrame(final_state, dt, frame_rate_andrea)
#~ Ps, Ns, freeEffectorsPerPhase = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, frame_rate_andrea)
#~ NPeffs = genPEffperFrame(fullBody, freeEffectorsPerPhase, new_traj_andrea, pp, time_per_path, frame_rate_andrea)
#~ com = genComPerFrame(final_state, dt, frame_rate_andrea)
#~ if(len(trajec) > 0):
#~ new_traj = new_traj[1:]
#~ new_traj_andrea = new_traj_andrea[1:]
......@@ -176,19 +176,19 @@ trackedEffectors = []):
#~ com = com[1:]
#~ NPeffs = NPeffs[1:]
trajec = trajec + new_traj
trajec_mil += new_traj_andrea
#~ trajec_mil += new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
global pos
pos += Ps
global normals
normals+= Ns
global pEffs
pEffs+= NPeffs
global coms
coms+= com
print len(trajec_mil), " ", len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs)
assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
#~ global pos
#~ pos += Ps
#~ global normals
#~ normals+= Ns
#~ global pEffs
#~ pEffs+= NPeffs
#~ global coms
#~ coms+= com
#~ print len(trajec_mil), " ", len(pos), " ", len(normals), " ", len(coms), " ", len(pEffs)
#~ assert(len(trajec_mil) == len(pos) and len(normals) == len(pos) and len(normals) == len(coms) and len(coms) == len(pEffs))
stat_data["num_success"] += 1
else:
print "TODO, NO CONTACT VARIATION, LINEAR INTERPOLATION REQUIRED"
......
src/rbprmbuilder.impl.cc
View file @ 790286d4
......@@ -812,6 +812,61 @@ namespace hpp {
}
}
hpp::floatSeq* RbprmBuilder::getContactSamplesIdsAndProject(const char* limbname,
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,
unsigned short numSamples) throw (hpp::Error)
{
if(!fullBodyLoaded_)
throw Error ("No full body robot was loaded");
try
{
fcl::Vec3f dir;
for(std::size_t i =0; i <3; ++i)
{
dir[i] = direction[(_CORBA_ULong)i];
}
model::Configuration_t config = dofArrayToConfig (fullBody_->device_, configuration);
model::Configuration_t save = fullBody_->device_->currentConfiguration();
fullBody_->device_->currentConfiguration(config);
sampling::T_OctreeReport finalSet;
rbprm::T_Limb::const_iterator lit = fullBody_->GetLimbs().find(std::string(limbname));
if(lit == fullBody_->GetLimbs().end())
{
throw std::runtime_error ("Impossible to find limb for joint "
+ std::string(limbname) + " to robot; limb not defined.");
}
const RbPrmLimbPtr_t& limb = lit->second;
fcl::Transform3f transform = limb->limb_->robot()->rootJoint()->childJoint(0)->currentTransformation (); // get root transform from configuration
// TODO fix as in rbprm-fullbody.cc!!
std::vector<sampling::T_OctreeReport> reports(problemSolver_->collisionObstacles().size());
std::size_t i (0);
//#pragma omp parallel for
for(model::ObjectVector_t::const_iterator oit = problemSolver_->collisionObstacles().begin();
oit != problemSolver_->collisionObstacles().end(); ++oit, ++i)
{
sampling::GetCandidates(limb->sampleContainer_, transform, *oit, dir, reports[i]);
}
for(std::vector<sampling::T_OctreeReport>::const_iterator cit = reports.begin();
cit != reports.end(); ++cit)
{
finalSet.insert(cit->begin(), cit->end());
}
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length((_CORBA_ULong)finalSet.size());
sampling::T_OctreeReport::const_iterator candCit = finalSet.begin();
for(std::size_t i=0; i< _CORBA_ULong(finalSet.size()); ++i, ++candCit)
{
(*dofArray)[(_CORBA_ULong)i] = (double)candCit->sample_->id_;
}
fullBody_->device_->currentConfiguration(save);
return dofArray;
} catch (const std::exception& exc) {
throw hpp::Error (exc.what ());
}
}
hpp::floatSeq* RbprmBuilder::getSamplesIdsInOctreeNode(const char* limb,
double octreeNodeId) throw (hpp::Error)
{
......
src/rbprmbuilder.impl.hh
View file @ 790286d4
......@@ -136,6 +136,11 @@ namespace hpp {
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction) throw (hpp::Error);
virtual hpp::floatSeq* getContactSamplesIdsAndProject(const char* limb,
const hpp::floatSeq& configuration,
const hpp::floatSeq& direction,
unsigned short numSamples) throw (hpp::Error);
virtual hpp::floatSeq* getSamplesIdsInOctreeNode(const char* limb,
double octreeNodeId) 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