lArmNormal = [1,0,0] lArmx = 0.024; lArmy = 0.024 fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, 0.01) #make sure this is 0 q_0 = fullBody.getCurrentConfig () q_0[0:7] = [0,0,0, 1, 0, 0, 0] fullBody.setCurrentConfig (q_0) import numpy as np effectorName = rfoot limbId = rLegId q = fullBody.getSample(limbId, 1) fullBody.setCurrentConfig(q) #setConfiguration matching sample qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix() #compute rotation matrix world -> local p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram rm=np.zeros((4,4)) for i in range(0,3): for j in range(0,3): rm[i,j] = rot[i,j] for i in range(0,3): rm[i,3] = qEffector[i] rm[3,3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0,0,0,1])
optim_effector=True, use_velocity=False, pathId=pId) # remove the last config (= user defined q_goal, not consitent with the previous state) #r(configs[0]) #player.displayContactPlan(1.) #player.interpolate(2,len(configs)-1) """ r(configs[5]) dir = configs[5][37:40] fullBody.client.rbprm.rbprm.evaluateConfig(configs[5],dir) """ from planning.config import * from generate_contact_sequence import * cs = generateContactSequence(fullBody, configs, r) filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE cs.saveAsXML(filename, "ContactSequence") print("save contact sequence : ", filename) r(q_init) pos = fullBody.getJointPosition('RLEG_JOINT0') addSphere(r, r.color.blue, pos) id = r.client.gui.getWindowID("window_hpp_") r.client.gui.attachCameraToNode('hrp2_14/BODY_0', id)
lArmx = 0.024 lArmy = 0.024 fullBody.addLimb(larmId, larm, lHand, lArmOffset, lArmNormal, lArmx, lArmy, nbSamples, 0.01) #make sure this is 0 q_0 = fullBody.getCurrentConfig() q_0[0:7] = [0, 0, 0, 1, 0, 0, 0] fullBody.setCurrentConfig(q_0) import numpy as np effectorName = rfoot limbId = rLegId q = fullBody.getSample(limbId, 1) fullBody.setCurrentConfig(q) #setConfiguration matching sample qEffector = fullBody.getJointPosition(effectorName) q0 = quat.Quaternion(qEffector[3:7]) rot = q0.toRotationMatrix() #compute rotation matrix world -> local p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram rm = np.zeros((4, 4)) for i in range(0, 3): for j in range(0, 3): rm[i, j] = rot[i, j] for i in range(0, 3): rm[i, 3] = qEffector[i] rm[3, 3] = 1 invrm = np.linalg.inv(rm) p = invrm.dot([0, 0, 0, 1]) def printRootPosition(limbId, effectorName, nbSamples):