Commit ca3bd590 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

correctly use hrp2 files in the fullmBodyPlayer

parent b4faf78f
......@@ -33,14 +33,14 @@ rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "EFORT_Normal", 0.1)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 50000, "manipulability", 0.1)
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "EFORT_Normal", 0.1)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 50000, "manipulability", 0.1)
......@@ -95,18 +95,29 @@ fullBody.setEndState(q_goal,[rLegId,lLegId])
configs = fullBody.interpolate(0.06,pathId=pId,robustnessTreshold = 1, filterStates = True)
configs = fullBody.interpolate(0.07,pathId=pId,robustnessTreshold = 3, filterStates = True)
print "number of configs :", len(configs)
r(configs[-1])
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = pId)
import fullBodyPlayerHrp2
player = fullBodyPlayerHrp2.Player(fullBody,pp,tp,configs,draw=False,use_window=1,optim_effector=False,use_velocity=True,pathId = pId)
#player.displayContactPlan()
player.interpolate(3,20)
player.displayContactPlan()
"""
reload(hpp.corbaserver.rbprm.tools.cwc_trajectory)
reload(hpp.corbaserver.rbprm.tools.cwc_trajectory_helper)
reload(fullBodyPlayerHrp2)
"""
......@@ -98,7 +98,7 @@ print "number of configs :", len(configs)
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = 0)
player.displayContactPlan()
......
from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
import time
class Player(object):
def __init__ (self, fullBody,pathPlayer,tp,configs=[],draw=False,use_window = 0 ,optim_effector=False,use_velocity=False,pathId = 0):
self.viewer = pathPlayer.publisher
self.tp = tp
self.pp = pathPlayer
self.fullBody=fullBody
self.configs=configs
self.rLegId = 'hrp2_rleg_rom'
self.lLegId = 'hrp2_lleg_rom'
self.rarmId = 'hrp2_rarm_rom'
self.draw=draw
self.pathId = pathId
self.use_velocity = use_velocity
self.optim_effector = optim_effector
self.use_window = use_window
self.limbsCOMConstraints = { self.rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
self.lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
self.rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : 'RARM_JOINT5'} }
def act(self,i, numOptim = 0, use_window = 0, friction = 0.5, optim_effectors = True, time_scale = 1, verbose = True, draw = False, trackedEffectors = []):
return step(self.fullBody, self.configs, i, numOptim, self.pp, self.limbsCOMConstraints, friction, optim_effectors = optim_effectors, time_scale = time_scale, useCOMConstraints = True, use_window = use_window,
verbose = verbose, draw = draw, trackedEffectors = trackedEffectors,use_velocity=self.use_velocity, pathId = self.pathId)
def initConfig(self):
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
if len(self.configs)>1:
self.viewer(self.configs[0])
else:
q_init = self.fullBody.getCurrentConfig()
q_init[0:7] = self.tp.ps.configAtParam(self.pathId,0.001)[0:7] # use this to get the correct orientation
dir_init = self.tp.ps.configAtParam(self.pathId,0.001)[7:10]
acc_init = self.tp.ps.configAtParam(self.pathId,0.001)[10:13]
# Randomly generating a contact configuration at q_init
self.fullBody.setCurrentConfig (q_init)
q_init = self.fullBody.generateContacts(q_init,dir_init,acc_init)
self.viewer(q_init)
def endConfig(self):
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
if len(self.configs)>1:
self.viewer(self.configs[len(self.configs)-1])
else:
q_goal = self.fullBody.getCurrentConfig()
q_goal[0:7] = self.tp.ps.configAtParam(self.pathId,self.tp.ps.pathLength(self.pathId))[0:7] # use this to get the correct orientation
dir_goal = self.tp.ps.configAtParam(self.pathId,0.01)[7:10]
acc_goal = self.tp.ps.configAtParam(self.pathId,0.01)[10:13]
# Randomly generating a contact configuration at q_init
self.fullBody.setCurrentConfig (q_goal)
q_goal = self.fullBody.generateContacts(q_goal,dir_goal,acc_goal)
self.viewer(q_goal)
def rootPath(self):
self.viewer.client.gui.setVisibility("hrp2_14", "OFF")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "ON")
self.tp.pp(self.pathId)
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
def genPlan(self):
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
start = time.clock()
self.configs = self.fullBody.interpolate(0.12, 10, 10, True)
end = time.clock()
print "Contact plan generated in " + str(end-start) + "seconds"
def displayContactPlan(self,pause = 0.5):
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
for i in range(0,len(self.configs)-1):
self.viewer(self.configs[i]);
time.sleep(pause)
def interpolate(self,begin=1,end=0):
if end==0:
end = len(self.configs) - 1
self.viewer.client.gui.setVisibility("hrp2_14", "ON")
self.tp.r.client.gui.setVisibility("toto", "OFF")
self.tp.r.client.gui.setVisibility("hrp2_trunk_flexible", "OFF")
for i in range(begin,end):
self.act(i,1,use_window=self.use_window,optim_effectors=self.optim_effector,draw=self.draw)
def play(self,frame_rate = 1./24.):
play_traj(self.fullBody,self.pp,frame_rate)
......@@ -26,21 +26,21 @@ ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps,viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "manipulability", 0.1)
lLegId = '1lLeg'
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "manipulability", 0.1)
rarmId = '3Rarm'
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
......@@ -139,7 +139,7 @@ print "number of configs :", len(configs)
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = 0)
player.displayContactPlan()
......
......@@ -26,21 +26,21 @@ ps = tp.ProblemSolver( fullBody )
r = tp.Viewer (ps,viewerClient=tp.r.client)
#~ AFTER loading obstacles
rLegId = '0rLeg'
rLegId = 'hrp2_rleg_rom'
rLeg = 'RLEG_JOINT0'
rLegOffset = [0,0,-0.105]
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 20000, "EFORT_Normal", 0.1)
lLegId = '1lLeg'
lLegId = 'hrp2_lleg_rom'
lLeg = 'LLEG_JOINT0'
lLegOffset = [0,0,-0.105]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 20000, "EFORT_Normal", 0.1)
rarmId = '3Rarm'
rarmId = 'hrp2_rarm_rom'
rarm = 'RARM_JOINT0'
rHand = 'RARM_JOINT5'
rArmOffset = [0,0,-0.1]
......@@ -139,7 +139,7 @@ print "number of configs :", len(configs)
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)
from fullBodyPlayer import Player
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=True,pathId = 0)
player.displayContactPlan()
......
......@@ -60,6 +60,7 @@ def __get_com_constraint(fullBody, state, config, limbsCOMConstraints, interm =
return [np.vstack(As), np.hstack(bs)]
def compute_state_info(fullBody,states, state_id, phase_dt, mu, computeCones, limbsCOMConstraints, pathId = 0):
print "phase dt in compute_state_info:",phase_dt
init_com = __get_com(fullBody, states[state_id])
end_com = __get_com(fullBody, states[state_id+1])
p, N = fullBody.computeContactPoints(state_id)
......@@ -285,7 +286,9 @@ def __optim__threading_ok(fullBody, states, state_id, computeCones = False, mu =
else:
res = gen_trajectory(fullBody, states, state_id, computeCones, mu, dt, phase_dt, reduce_ineq, verbose, limbsCOMConstraints, profile, use_window,use_velocity, pathId)
alpha = res[1]['alpha']
print "t in optim_threading :",res[1]["t_init_phases"]
t = [ti * alpha for ti in res[1]["t_init_phases"]]
print "t after alpha in optim_threading :",t
dt = res[1]["dt"] * alpha
final_state = res[0]
c0 = res[1]["x_init"][0:3]
......
......@@ -185,7 +185,7 @@ trackedEffectors = [],use_velocity=False,pathId = 0):
res = res + [pid]
global trajec
global trajec_mil
frame_rate = 1./60
frame_rate = 1./24.
frame_rate_andrea = 1./100.
# frame_rate_andrea = 1./1000.
#~ if(len(trajec) > 0):
......
......@@ -170,7 +170,7 @@ namespace hpp {
(RbPrmFullBodyPtr_t, core::ProblemPtr_t, const core::PathPtr_t,
const State &, const State &, const std::size_t, const bool);
hpp::floatSeq* rrt(t_rrt functor ,double state1,
hpp::floatSeq* rrt(t_rrt functor , double state1,
unsigned short comTraj1, unsigned short comTraj2, unsigned short comTraj3,
unsigned short numOptimizations) 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