#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db") ## Add arms (not used for contact) : rarmId = 'hrp2_rarm_rom' rarm = 'RARM_JOINT0' rHand = 'RARM_JOINT5' fullBody.addNonContactingLimb(rarmId, rarm, rHand, 10000) fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True) larmId = 'hrp2_larm_rom' larm = 'LARM_JOINT0' lHand = 'LARM_JOINT5' fullBody.addNonContactingLimb(larmId, larm, lHand, 10000) fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True) fullBody.setReferenceConfig(q_ref) # must be called after adding the limbs tGenerate = time.time() - tStart print "generate databases in : " + str(tGenerate) + " s" """ fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward") fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward") tLoad = time.time() - tStart print "Load databases in : "+str(tLoad)+" s" """ q_0 = fullBody.getCurrentConfig() #~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,) eps = 0.0001 configSize = fullBody.getConfigSize( ) - fullBody.client.basic.robot.getDimensionExtraConfigSpace()
load_paths("19_06_p") save_paths("19_06_p_save") save("19_06_s_save") def sac(): save("19_06_s") save_paths("19_06_p") init_bezier_traj(fullBody, r, pp, [], limbsCOMConstraints) all_paths = [[], []] from hpp.corbaserver.rbprm.state_alg import * #~ d(0.07);e(0.01) i = 0 d(0.1) e(0.01) states = planToStates(fullBody, configs) #~ lc() #~ fullBody.setReferenceConfig(configs[-1]) fullBody.setReferenceConfig(states[0].q()) onepath2(states[0:-1], nopt=0, mu=1, effector=False, init_acc=[0, 0, 0.], init_vel=[0., 0., 0.]) plall()