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