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):