#~ fullBody.client.rbprm.rbprm.getOctreeTransform(larmId, q_0)
#~ problem = ps.client.problem
#~ length = problem.pathLength (0)
#~ t = 0
#~ i = 0
#~ configs = []
#~ dt = 0.1 / length
#~ while t < length :
	#~ q = fullBody.getCurrentConfig()
	#~ q[0:confsize] = problem.configAtParam (0, t)[0:confsize]
	#~ configs.append(q)
	#~ t += dt
	#~ i = i+1
	#~ 
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1

import datetime
now = datetime.datetime.now()
#~ 
f1 = open("polaris_hrp2"+str(now),"w+")
f1.write(str(configs))
f1.close()

#~ import hpp.gepetto.blender.exportmotion as em

#~ fullBody.exportAll(r, configs, "polaris_hrp2_OK"+str(now));
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client.basic, r)

        0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0,               # RLEG       31-36
        ]; r (q_init)

fullBody.setCurrentConfig (q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])
#~ r(q_goal)

fullBody.setStartState(q_init,[rLegId,lLegId]) #,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId])#,rarmId,larmId])
#~ 
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0;
fullBody.draw(configs[i],r); i=i+1; i-1

#~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")

limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},  
						lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'}, 
						rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand},
						larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand} }

from hpp.gepetto import PathPlayer
#~ pp = PathPlayer (ps.robot.client.basic, r, 0.001, 0.1)
pp = PathPlayer (ps.robot.client.basic, r)
#~ (self, client, publisher, dt=0.01, speed=1)

from hpp.corbaserver.rbprm.tools.cwc_trajectory_helper import step, clean,stats, saveAllData, play_traj
]
r(q_init)

fullBody.setCurrentConfig(q_goal)
#~ r(q_goal)
q_goal = fullBody.generateContacts(q_goal, [0, 0, 1])
#~ r(q_goal)

fullBody.setStartState(q_init, [rLegId, lLegId])  #,rarmId,larmId])
fullBody.setEndState(q_goal, [rLegId, lLegId])  #,rarmId,larmId])
#~
#~ configs = fullBody.interpolate(0.1)
configs = fullBody.interpolate(0.1)
#~ configs = fullBody.interpolate(0.15)
i = 0
fullBody.draw(configs[i], r)
i = i + 1
i - 1

#~ r.loadObstacleModel ('hpp-rbprm-corba', "stair_bauzil", "contact")

limbsCOMConstraints = {
    rLegId: {
        'file': "hrp2/RL_com.ineq",
        'effector': 'RLEG_JOINT5'
    },
    lLegId: {
        'file': "hrp2/LL_com.ineq",
        'effector': 'LLEG_JOINT5'
    },
    rarmId: {
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])

# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig (q_goal)
q_goal = fullBody.generateContacts(q_goal, [0,0,1])

# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])


r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.1, 1, 0)
# RB: Is this line redundent?
r.loadObstacleModel ('hpp-rbprm-corba', name_of_scene, "contact")

i = 0;
# fullBody.draw(configs[i],r); i=i+1; i-1

while (i < len(configs)):
	fullBody.draw(configs[i],r)
	sleep(0.1)
	i = i + 1

print "Animation finished!"