Commit 6dccc4b4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

new api for pinocchio trajectory export

parent 1d77cb63
......@@ -228,6 +228,11 @@ module hpp
/// \param contactLimbs ids of the limb in contact for the state
void setStartState(in floatSeq dofArray, in Names_t contactLimbs) raises (Error);
/// Compute effector contact points and normals for a given configuration
/// in local coordinates
/// \param dofArray configuration of the robot
/// \param limbName ids of the limb in contact
floatSeq computeContactForConfig(in floatSeq dofArray, in string limbName) raises (Error);
/// Set the end state of a contact generation problem
/// environment, ordered by their efficiency
......@@ -241,11 +246,11 @@ module hpp
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPoints(in unsigned short stateId) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state, the next one, and the intermediate between them.
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
/// Provided a discrete contact sequence has already been computed, computes
/// all the contact positions and normals for a given state, the next one, and the intermediate between them.
/// \param stateId normalized step for generation along the path (ie the path has a length of 1).
/// \return list of 2 or 3 lists of 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
floatSeqSeq computeContactPointsForLimb(in unsigned short stateId, in string limbname) raises (Error);
/// Provided a path has already been computed, interpolates it and generates the statically stable
/// constact configurations along it. setStartState and setEndState must have been called prior
......
......@@ -307,6 +307,16 @@ class FullBody (object):
def computeContactPointsForLimb(self, stateId, limb):
rawdata = self.client.rbprm.rbprm.computeContactPointsForLimb(stateId, limb)
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]
## Compute effector contact points and normals for a given configuration
# in local coordinates
#
# \param q configuration of the robot
# \param limbName ids of the limb in contact
# \return list 6d vectors [pox, poy, posz, nprmalx, normaly, normalz]
def computeContactForConfig(q, limbName):
rawdata = self.computeContactPointsForLimb(stateId, limb)
return [[rawdata[i:i+3]] for i in range(0, len(rawdata), 6)], [[rawdata[i+3:i+6]] for i in range(0, len(rawdata), 6)]
## Provided a discrete contact sequence has already been computed, computes
# all the contact positions and normals for a given state, the next one, and the intermediate between them.
......
......@@ -129,13 +129,16 @@ reduce_ineq = True, verbose = False, limbsCOMConstraints = None, profile = False
timeelapsed = (end - start) * 1000
#~ print "solving time", timeelapsed
if(use_window > 0):
var_final['c_old'] = var_final['c'][:]
var_final['dc_old'] = var_final['dc'][:]
var_final['ddc_old'] = var_final['ddc'][:]
var_final['c'] = var_final['c'][:init_waypoint_time+1]
var_final['dc'] = var_final['dc'][:init_waypoint_time+1]
var_final['ddc'] = var_final['ddc'][:init_waypoint_time+1]
params["t_init_phases"] = params["t_init_phases"][:-3*use_window]
print "trying to project on com (from, to) ", init_end_com, var_final['c'][-1]
if (fullBody.projectStateToCOM(state_id+1, (var_final['c'][-1]).tolist())):
print "PROJECTED", init_end_com, var_final['c'][-1]
#~ print "PROJECTED", init_end_com, var_final['c'][-1]
states[state_id+1] = fullBody.getConfigAtState(state_id+1) #updating config from python side)
lastspeed = var_final['dc'][init_waypoint_time]
print "init speed", lastspeed
......@@ -182,9 +185,13 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
print "end target ", params['x_end']
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['dc']
if(use_window > 0):
points = var_final['dc_old']
else:
points = var_final['dc']
#~ print "points", points
ys = [norm(el) * el[0] / abs(el[0]) for el in points]
ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
......@@ -194,14 +201,25 @@ def draw_trajectory(fullBody, states, state_id, computeCones = False, mu = 1, d
print "plotting acceleration "
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['ddc']
ys = [norm(el) * el[0] / abs(el[0]) for el in points]
if(use_window > 0):
points = var_final['ddc_old']
else:
points = var_final['ddc']
ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
plt.show()
print "plotting Dl "
fig = plt.figure()
ax = fig.add_subplot(111)
points = var_final['dL']
ys = [norm(el) * el[0] / abs(el[0]+ _EPS) for el in points]
xs = [i * params['dt'] for i in range(0,len(points))]
ax.scatter(xs, ys, c='b')
plt.show()
return var_final, params, elapsed
......@@ -241,20 +259,20 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
#now compute com trajectorirs
comTrajIds = [fullBody.generateComTraj(comPosPerPhase[i], comVelPerPhase[i], comAccPerPhase[i], dt) for i in range(0,3)]
return comTrajIds, res[2] #res[2] is timeelapsed
return comTrajIds, res[2], final_state #res[2] is timeelapsed
def solve_com_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
paths_ids = [int(el) for el in fullBody.comRRTFromPos(state_id,comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims)]
print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
def solve_effector_RRT(fullBody, states, state_id, computeCones = False, mu = 1, dt =0.1, phase_dt = [0.4, 1],
reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConstraints = None, profile = False, use_window = 0, trackedEffectors = []):
comPosPerPhase, timeElapsed = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
comPosPerPhase, timeElapsed, final_state = __optim__threading_ok(fullBody, states, state_id, computeCones, mu, dt, phase_dt,
reduce_ineq, num_optims, draw, verbose, limbsCOMConstraints, profile, use_window)
print "done. generating state trajectory ",state_id
if(len(trackedEffectors) == 0):
......@@ -264,5 +282,5 @@ reduce_ineq = True, num_optims = 0, draw = False, verbose = False, limbsCOMConst
refPathId = trackedEffectors[0]; path_start = trackedEffectors[1]; path_to = trackedEffectors[2]; effectorstracked = trackedEffectors[3]
paths_ids = [int(el) for el in fullBody.effectorRRTFromPath(state_id, refPathId, path_start, path_to, comPosPerPhase[0],comPosPerPhase[1],comPosPerPhase[2],num_optims, effectorstracked)]
print "done. computing final trajectory to display ",state_id, "path ids ", paths_ids[-1], " ," , paths_ids[:-1]
return paths_ids[-1], paths_ids[:-1], timeElapsed
return paths_ids[-1], paths_ids[:-1], timeElapsed, final_state
......@@ -3,7 +3,7 @@ from hpp.corbaserver.rbprm.tools.cwc_trajectory import *
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
from cwc import OptimError, cone_optimization
from hpp.corbaserver.rbprm.tools.path_to_trajectory import gen_trajectory_to_play
from numpy import append
from numpy import append, array
#global variables
res = []
......@@ -12,6 +12,8 @@ trajec_mil = []
#~ contacts = []
pos = []
normals = []
pEffs = []
coms = []
errorid = []
def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
......@@ -29,43 +31,10 @@ def displayComPath(pp, pathId,color=[0.,0.75,0.15,0.9]) :
pp.publisher.client.gui.addToGroup(nameCurve,pp.publisher.sceneName)
pp.publisher.client.gui.refresh()
#~
#~ def getContactPerPhase(fullBody, stateid, limbsCOMConstraints):
#~ contacts = [[],[],[]]
#~ for k, v in limbsCOMConstraints.iteritems():
#~ if(fullBody.isLimbInContact(k, stateid)):
#~ contacts[0]+=[v['effector']]
#~ if(fullBody.isLimbInContactIntermediary(k, stateid)):
#~ contacts[1]+=[v['effector']]
#~ if(fullBody.isLimbInContact(k, stateid+1)):
#~ contacts[2]+=[v['effector']]
#~ return contacts
#~
#~ def gencontactsPerFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
#~ contactsPerPhase = getContactPerPhase(fullBody, stateid, limbsCOMConstraints)
#~ config_size = len(fullBody.getCurrentConfig())
#~ interpassed = False
#~ res = []
#~ for path_id in path_ids:
#~ length = pp.client.problem.pathLength (path_id)
#~ num_frames_required_fly = times[1] / dt_framerate
#~ num_frames_required_support = times[0] / dt_framerate
#~ dt_fly = float(length) / num_frames_required_fly
#~ dt_support = float(length) / num_frames_required_support
#~ dt_finals_fly = [dt_fly*i for i in range(int(num_frames_required_fly))] + [1]
#~ dt_finals_support = [dt_support*i for i in range(int(num_frames_required_support))] + [1]
#~ config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
#~ if(config_size_path > config_size):
#~ interpassed = True
#~ res+= [contactsPerPhase[1] for t in dt_finals_fly]
#~ elif interpassed:
#~ res+= [contactsPerPhase[2] for t in dt_finals_support]
#~ else:
#~ res+= [contactsPerPhase[0] for t in dt_finals_support]
#~ return res
def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times, dt_framerate=1./24.):
p, N= fullBody.computeContactPointsPerLimb(stateid, limbsCOMConstraints.keys(), limbsCOMConstraints)
freeEffectors = [ [limbsCOMConstraints[limb]['effector'] for limb in limbsCOMConstraints.keys() if not p[i].has_key(limbsCOMConstraints[limb]['effector'])] for i in range(len(p))]
config_size = len(fullBody.getCurrentConfig())
interpassed = False
pRes = []
......@@ -73,24 +42,45 @@ def genPandNperFrame(fullBody, stateid, limbsCOMConstraints, pp, path_ids, times
for idx, path_id in enumerate(path_ids):
length = pp.client.problem.pathLength (path_id)
num_frames_required = times[idx] / dt_framerate
print "dt_framerate", dt_framerate
print "num_frames_required", times[idx], " ", num_frames_required
#~ print "dt_framerate", dt_framerate
#~ print "num_frames_required", times[idx], " ", num_frames_required
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [1]
#~ config_size_path = len(pp.client.problem.configAtParam (path_id, 0))
#~ if(config_size_path > config_size):
#~ interpassed = True
dt_finals = [dt*i for i in range(int(round(num_frames_required)))]
pRes+= [p[idx] for t in dt_finals]
nRes+= [N[idx] for t in dt_finals]
#~ elif interpassed:
#~ pRes+= [p[2] for t in dt_finals_support]
#~ nRes+= [N[2] for t in dt_finals_support]
#~ else:
#~ pRes+= [p[0] for t in dt_finals_support]
#~ nRes+= [N[0] for t in dt_finals_support]
return pRes, nRes
return pRes, nRes, freeEffectors
def __getPos(effector, fullBody, config):
fullBody.setCurrentConfig (config)
q = fullBody.getJointPosition(effector)
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
return q
def genPEffperFrame(fullBody, freeEffectorsPerPhase, qs, pp, times, dt_framerate):
res = []
for idx, phase in enumerate(freeEffectorsPerPhase):
num_frames_required = int(times[idx] / dt_framerate)
qid = len(res)
for q in qs[qid:num_frames_required+qid]:
p = {}
for effector in phase:
p[effector] = __getPos(effector, fullBody, q)
res.append(p)
return res
def genComPerFrame(final_state, dt, dt_framerate = 1./1000.):
num_frames_per_dt = int(round(dt / dt_framerate))
c = [array(final_state['x_init'][:3])] + final_state['c']
dc = [array(final_state['x_init'][3:])] + final_state['dc']
ddc = final_state['ddc']
cs = []
[cs.append(c[i] + ddt*dt *dc[i] + ddt*dt *ddt* dt * 0.5 * ddc[i]) for ddt in range(num_frames_per_dt) for i in range(0,len(ddc))]
return cs
stat_data = {
"error_com_proj" : 0,
"error_optim_fail" : 0,
......@@ -154,9 +144,9 @@ trackedEffectors = []):
else:
comC = None
if(optim_effectors):
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
pid, trajectory, timeelapsed, final_state = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
else :
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
pid, trajectory, timeelapsed, final_state = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, draw, verbose, comC, False, use_window=use_window, trackedEffectors = trackedEffectors)
displayComPath(pp, pid)
#~ pp(pid)
global res
......@@ -171,24 +161,31 @@ trackedEffectors = []):
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_contacts = gencontactsPerFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, times, frame_rate_andrea)
Ps, Ns = genPandNperFrame(fullBody, i, limbsCOMConstraints, pp, trajectory, time_per_path, 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:]
#~ new_contacts = new_contacts[1:]
Ps = Ps[1:]
Ns = Ns[1:]
com = com[1:]
pEffs = pEffs[1:]
trajec = trajec + new_traj
trajec_mil += new_traj_andrea
#~ global contacts
#~ contacts += new_contacts
global pos
print "pos", len(pos), " ps, ", len(Ps)
pos += Ps
global normals
normals+= Ns
print len(trajec_mil), " ", len(pos), " ", len(normals)
assert(len(trajec_mil) == len(pos) and len(normals) == len(pos))
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"
......@@ -289,9 +286,9 @@ def step_profile(fullBody, configs, i, optim, limbsCOMConstraints, friction = 0
else:
comC = None
if(optim_effectors):
pid, trajectory, timeelapsed = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
pid, trajectory, timeelapsed, final_state = solve_effector_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
else :
pid, trajectory, timeelapsed = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
pid, trajectory, timeelapsed, final_state = solve_com_RRT(fullBody, configs, i, True, friction, dt, times, False, optim, False, False, comC, True)
__update_cwc_time(timeelapsed)
stat_data["num_success"] += 1
else:
......@@ -346,7 +343,31 @@ def displayInSave(pp, pathId, configs):
import time
def __isDiff(P0, P1):
return len(set(P0.keys()) - set(P1.keys())) != 0 or len(set(P1.keys()) - set(P0.keys()))
from pickle import dump
def compressData(data_array, filename):
qs = [data['q'][:] for data in data_array]
C = [data['C'][:] for data in data_array]
a = {}
frameswitches = []
for i in range(0,len(pos)):
if i == 0 or __isDiff(pos[i], pos[i-1]):
a = {}
for effector in pos[i].keys():
a[effector] = {'P' : pos[i][effector], 'N' : normals[i][effector]}
frameswitches.append([i,a])
res = {}
res['Q'] = [data['q'][:] for data in data_array]
res['C'] = [data['C'][:] for data in data_array]
res['fly'] = pEffs
res['frameswitches'] = frameswitches
f1=open(filename+"_compressed", 'w+')
dump(res, f1)
f1.close()
return res
def saveToPinocchio(filename):
res = []
for i, q_gep in enumerate(trajec_mil):
......@@ -355,12 +376,12 @@ def saveToPinocchio(filename):
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
data = {'q':q, 'P' : pos[i], 'N' : normals[i]}
data = {'q':q, 'P' : pos[i], 'N' : normals[i], 'C' : coms [i], 'pEffs' : pEffs[i]}
res += [data]
f1=open(filename, 'w+')
dump(res, f1)
f1.close()
return res
return compressData(res,filename)
def clean():
global res
......@@ -370,6 +391,8 @@ def clean():
global errorid
global pos
global normals
global pEffs
global coms
res = []
trajec = []
trajec_mil = []
......@@ -377,6 +400,8 @@ def clean():
errorid = []
pos = []
normals = []
pEffs = []
coms = []
import copy
......@@ -413,7 +438,7 @@ def profile(fullBody, configs, i_start, i_end, limbsCOMConstraints, friction =
def saveAllData(fullBody, r, name):
fullBody.exportAll(r, trajec, name)
saveToPinocchio(name)
return saveToPinocchio(name)
def play_traj(fullBody,pp,frame_rate):
global trajec
......
__24fps = 1. / 24.
__EPS = 0.0000001
__EPS = 0.00001
from numpy.linalg import norm
def __linear_interpolation(p0,p1,dist_p0_p1, val):
......@@ -9,7 +9,7 @@ def __gen_frame_positions(com_waypoints, dt, dt_framerate=__24fps):
total_time = (len(com_waypoints) - 1) * dt
dt_final = total_time * dt_framerate
num_frames_required = total_time / dt_framerate + 1
dt_finals = [dt_final*i for i in range(int(num_frames_required))]
dt_finals = [dt_final*i for i in range(int(round(num_frames_required)))]
ids_val = []
for i in range(len(com_waypoints) - 1):
ids_val += [(i,val-dt*i) for val in dt_finals if (val < (i+1) *dt) and (val > i*dt)]
......@@ -44,7 +44,7 @@ def linear_interpolate_path(robot, path_player, path_id, total_time, dt_framerat
length = pp.client.problem.pathLength (path_id)
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
dt_finals = [dt*i for i in range(int(num_frames_required))] + [length]
dt_finals = [dt*i for i in range(int(round(num_frames_required))-1)] + [length]
return[pp.client.problem.configAtParam (path_id, t) for t in dt_finals]
def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate=__24fps):
......@@ -53,8 +53,8 @@ def follow_trajectory_path(robot, path_player, path_id, total_time, dt_framerate
num_frames_required = total_time / dt_framerate
dt = float(length) / num_frames_required
#matches the extradof normalized
print "length ", length, "total tome ", total_time, "frame rate ", dt_framerate, "num_frames_required ", num_frames_required, "dt ", dt
dt_finals = [dt*i / length for i in range(int(num_frames_required))] + [1]
#~ print "length ", length, "total tome ", total_time, "frame rate ", dt_framerate, "num_frames_required ", num_frames_required, "dt ", dt
dt_finals = [dt*i / length for i in range(int(round(num_frames_required)))]
return[__find_q_t(robot, path_player, path_id, t) for t in dt_finals]
def gen_trajectory_to_play(robot, path_player, path_ids, total_time_per_paths, dt_framerate=__24fps):
......
......@@ -845,13 +845,12 @@ namespace hpp {
void SetPositionAndNormal(rbprm::State& state,
hpp::rbprm::RbPrmFullBodyPtr_t fullBody, const hpp::floatSeq& configuration,
const hpp::Names_t& contactLimbs)
std::vector<std::string>& names)
{
core::Configuration_t old = fullBody->device_->currentConfiguration();
model::Configuration_t config = dofArrayToConfig (fullBody->device_, configuration);
fullBody->device_->currentConfiguration(config);
fullBody->device_->computeForwardKinematics();
std::vector<std::string> names = stringConversion(contactLimbs);
for(std::vector<std::string>::const_iterator cit = names.begin(); cit != names.end();++cit)
{
rbprm::T_Limb::const_iterator lit = fullBody->GetLimbs().find(*cit);
......@@ -865,7 +864,8 @@ namespace hpp {
const fcl::Matrix3f& rot = transform.getRotation();
state.contactPositions_[*cit] = transform.getTranslation();
state.contactRotation_[*cit] = rot;
state.contactNormals_[*cit] = fcl::Vec3f(rot(0,2),rot(1,2), rot(2,2));
const fcl::Vec3f z = transform.getRotation() * lit->second->normal_;
state.contactNormals_[*cit] = z;
state.contacts_[*cit] = true;
state.contactOrder_.push(*cit);
}
......@@ -880,7 +880,8 @@ namespace hpp {
{
try
{
SetPositionAndNormal(startState_,fullBody_, configuration, contactLimbs);
std::vector<std::string> names = stringConversion(contactLimbs);
SetPositionAndNormal(startState_,fullBody_, configuration, names);
}
catch(std::runtime_error& e)
{
......@@ -888,11 +889,43 @@ namespace hpp {
}
}
hpp::floatSeq* RbprmBuilder::computeContactForConfig(const hpp::floatSeq& configuration, const char *limbNam) throw (hpp::Error)
{
State state;
std::string limb(limbNam);
try
{
std::vector<std::string> limbs; limbs.push_back(limbNam);
SetPositionAndNormal(state,fullBody_, configuration, limbs);
const std::vector<fcl::Vec3f>& positions = computeRectangleContact(fullBody_,state,limb);
_CORBA_ULong size = (_CORBA_ULong) positions.size () * 3;
hpp::floatSeq* dofArray = new hpp::floatSeq();
dofArray->length(size);
for(std::size_t h = 0; h<positions.size(); ++h)
{
for(std::size_t k =0; k<3; ++k)
{
model::size_type j (h*3 + k);
dofArray[j] = positions[h][k];
}
}
return dofArray;
}
catch(std::runtime_error& e)
{
throw Error(e.what());
}
}
void RbprmBuilder::setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error)
{
try
{
SetPositionAndNormal(endState_,fullBody_, configuration, contactLimbs);
std::vector<std::string> names = stringConversion(contactLimbs);
SetPositionAndNormal(endState_,fullBody_, configuration, names);
}
catch(std::runtime_error& e)
{
......
......@@ -147,6 +147,7 @@ namespace hpp {
virtual void setStartState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual void setEndState(const hpp::floatSeq& configuration, const hpp::Names_t& contactLimbs) throw (hpp::Error);
virtual hpp::floatSeq* computeContactForConfig(const hpp::floatSeq& configuration, const char* limbNam) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPoints(unsigned short cId) throw (hpp::Error);
virtual hpp::floatSeqSeq* computeContactPointsForLimb(unsigned short cId, const char* limbName) throw (hpp::Error);
virtual hpp::floatSeqSeq* interpolate(double timestep, double path, double robustnessTreshold, unsigned short filterStates) 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