nbSamples, "EFORT", 0.01)
larmId = "larm"
larm = "arm_left_1_joint"
lHand = "gripper_left_inner_single_joint"
lArmOffset = [0, 0, -0.1]
lArmNormal = [0, 0, 1]
lArmx = 0.02
lArmy = 0.02
fullBody.addLimb(larmId, larm, lHand, lArmOffset, lArmNormal, lArmx, lArmy,
nbSamples, "EFORT", 0.01)
zeroConf = [0, 0, 0, 1, 0, 0, 0]
q_0[0:7] = zeroConf
fullBody.setCurrentConfig(q_0)
effectors = [rfoot, lfoot, lHand, rHand]
limbIds = [rLegId, lLegId, larmId, rarmId]
# 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):
dir_init = tp.ps.configAtParam(pathId, 0.01)[7:10]
acc_init = tp.ps.configAtParam(pathId, 0.01)[10:13]
dir_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[7:10]
acc_goal = tp.ps.configAtParam(pathId, tp.ps.pathLength(pathId))[10:13]
configSize = fullBody.getConfigSize(
) - fullBody.client.basic.robot.getDimensionExtraConfigSpace()
# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = dir_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = dir_goal[::]
q_goal[configSize + 3:configSize + 6] = acc_goal[::]
fullBody.setStaticStability(False)
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig(q_init)
q_init = fullBody.generateContacts(q_init, dir_init, acc_init, 2)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig(q_goal)
q_goal = fullBody.generateContacts(q_goal, dir_goal, acc_goal, 2)
# specifying the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init, [larmId, rLegId, rarmId, lLegId])
fullBody.setEndState(q_goal, [larmId, rLegId, rarmId, lLegId])
r(q_init)
# computing the contact sequence
configs = fullBody.interpolate(0.001,
pathId=pathId,
rLegId = 'RFoot'
lLegId = 'LFoot'
rarmId = 'RHand'
larmId = 'LHand'
nbSamples = 50000
x = 0.03
y = 0.08
fullBody.addLimb(rLegId, 'RThigh_rx', 'SpidermanRFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(lLegId, 'LThigh_rx', 'SpidermanLFootSphere', [0, 0, 0],
[0, 0, 1], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(rarmId, 'RHumerus_rx', 'SpidermanRHandSphere', [0, 0, 0],
[0, -1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
fullBody.addLimb(larmId, 'LHumerus_rx', 'SpidermanLHandSphere', [0, 0, 0],
[0, 1, 0], x, y, nbSamples, "EFORT_Normal", 0.01, "_6_DOF")
print("Limbs added to fullbody")
confsize = len(tp.q11)
### TEST OF CONTACT CREATION FOR INTERMEDIATE CONFIG, NOT USED FOR INTERPOLATION
q_tmp = q_contact_takeoff[::]
q_tmp[0:confsize - tp.ecsSize] = tp.q_cube[0:confsize - tp.ecsSize]
rr(q_tmp)
fullBody.setCurrentConfig(q_tmp)
q_tmp_test = fullBody.generateContacts(q_tmp, [0, -1, 0], True)
rr(q_tmp_test)
fullBody.isConfigValid(q_tmp_test)
# result:
# q_tmp_test = [-1.2, -2.8, 3.6, 0.707107, 0.0, 0.0, 0.707107, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.9324800803082636, -0.9184709614284768, 0.6886200849241174, -0.6066622060535428, 0.47649495495305294, 1.0976823065116303, -0.538404483799899, -1.0681738092891575, -1.1021076588270258, 1.1498838725595328, -0.6156809000975677, 0.5815958533218022, -1.4659758542959642, -0.3603605133380307, 0.36116581520970376, -1.048638878548546, 0.24059108997189355, 1.23953255675232, -0.7519812787252685, -0.1402404928640359, -1.0, 0.023118656707620415, -0.6298340889273957, -0.15800407650545129, 0.4963824557225752, -0.4989080182494368, 0.2774303858753873, -0.9974339561414656]
rarmId = 'rhleg'
rarm = 'rh_haa_joint'
rHand = 'rh_foot_joint'
fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
larmId = 'lfleg'
larm = 'lf_haa_joint'
lHand = 'lf_foot_joint'
fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "jointlimits", 0.05, cType)
q_0 = fullBody.getCurrentConfig();
q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.q_init[0:7]
q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.q_goal[0:7]
# Randomly generating a contact configuration at q_init
fullBody.setCurrentConfig (q_init)
q_init = fullBody.generateContacts(q_init, [0,0,1])
#~ from pickle import load
#~ f = open("config_"+str(tp.config_i), 'r')
#~ q_init = load(f)
#~ f.close()
# 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.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
rArmx = 0.024; rArmy = 0.024 fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, 0.01) larmId = 'lArm' larm = 'LARM_JOINT0' lHand = 'LARM_JOINT5' lArmOffset = [-0.05,-0.050,-0.050] 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):
nbSamples, 0.01)
larmId = 'lArm'
larm = 'LARM_JOINT0'
lHand = 'LARM_JOINT5'
lArmOffset = [-0.05, -0.050, -0.050]
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):
rarmId = 'rhleg' rarm = 'rh_haa_joint' rHand = 'rh_foot_Z' fullBody.addLimb(rarmId,rarm,rHand,offset,normal, legx, legy, nbSamples, "manipulability", 0.05, cType) larmId = 'lfleg' larm = 'lf_haa_joint' lHand = 'lf_foot_Z' fullBody.addLimb(larmId,larm,lHand,offset,normal, legx, legy, nbSamples, "forward", 0.05, cType) #make sure this is 0 q_0 = fullBody.getCurrentConfig () zeroConf = [0,0,0, 1, 0, 0, 0] q_0[0:7] = zeroConf fullBody.setCurrentConfig (q_0) effectors = [rfoot, lfoot, lHand, rHand] limbIds = [rLegId, lLegId, larmId, rarmId ] 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):
