Skip to content
GitLab
Commit 9dc2c37f authored by Steve Tonneau's avatar Steve Tonneau
Browse files

gen_com_pos_per_effector

parent 4022d15f
Hide whitespace changes
Inline Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @ 9dc2c37f
......@@ -68,12 +68,12 @@ lArmx = 0.015; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, n_samples, "manipulability", 0.05, "_6_DOF", True)
limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
larmId : {'file': "hrp2/LA_com.ineq", 'effector' : lHand},
rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~ limbsCOMConstraints = { rLegId : {'file': "hrp2/RL_com.ineq", 'effector' : 'RLEG_JOINT5'},
#~ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~ lLegId : {'file': "hrp2/LL_com.ineq", 'effector' : 'LLEG_JOINT5'},
#~ rarmId : {'file': "hrp2/RA_com.ineq", 'effector' : rHand} }
#~
......@@ -84,249 +84,262 @@ import quaternion as quat
def _getTransform(qEffector):
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix() #compute rotation matrix local -> world
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
rm=np.zeros((4,4))
for k in range(0,3):
for l in range(0,3):
rm[k,l] = rot[k,l]
for m in range(0,3):
rm[m,3] = qEffector[m]
rm[3,3] = 1
return rm
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix() #compute rotation matrix local -> world
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
rm=np.zeros((4,4))
for k in range(0,3):
for l in range(0,3):
rm[k,l] = rot[k,l]
for m in range(0,3):
rm[m,3] = qEffector[m]
rm[3,3] = 1
return rm
from numpy.linalg import norm
def __loosely_z_aligned(limb, config):
fullBody.setCurrentConfig(config)
effectorName = limbsCOMConstraints[limb]['effector']
m = _getTransform(fullBody.getJointPosition(effectorName))
P, N = fullBody.computeContactForConfig(config, limb)
#~ N_world = m.dot(array(N[0]+[1]))[:3]
N_world = m[:3,:3].dot(array(N[0]))
N_world = N_world / np.linalg.norm(N_world)
return N_world.dot(array([0,0,1])) > 0.7
fullBody.setCurrentConfig(config)
effectorName = limbsCOMConstraints[limb]['effector']
m = _getTransform(fullBody.getJointPosition(effectorName))
P, N = fullBody.computeContactForConfig(config, limb)
#~ N_world = m.dot(array(N[0]+[1]))[:3]
N_world = m[:3,:3].dot(array(N[0]))
N_world = N_world / np.linalg.norm(N_world)
return N_world.dot(array([0,0,1])) > 0.7
def draw_cp(cid, limb, config):
global r
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(config, limb)
fullBody.setCurrentConfig(config)
effectorName = limbsCOMConstraints[limb]['effector']
limbId = limb
m = _getTransform(fullBody.getJointPosition(effectorName))
scene = "qds"+limb+str(cid)
r.client.gui.createScene(scene)
for i in range(4):
#~ pos = posetc[2*i]
print "P", array(P[i]+[1])
print "N", array(N[i]+[1])
print m.dot(array(P[i]+[1]))
pos = m.dot(array(P[i]+[1]))[:3]
print "pos", pos
r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos.tolist()+[1,0,0,0])
r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0)
global r
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(config, limb)
fullBody.setCurrentConfig(config)
effectorName = limbsCOMConstraints[limb]['effector']
limbId = limb
m = _getTransform(fullBody.getJointPosition(effectorName))
scene = "qds"+limb+str(cid)
r.client.gui.createScene(scene)
for i in range(4):
#~ pos = posetc[2*i]
print "P", array(P[i]+[1])
print "N", array(N[i]+[1])
print m.dot(array(P[i]+[1]))
pos = m.dot(array(P[i]+[1]))[:3]
print "pos", pos
r.client.gui.addBox(scene+"/b"+str(i),0.01,0.01,0.01, [1,0,0,1])
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos.tolist()+[1,0,0,0])
r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0)
def fill_contact_points(limbs, config, config_pinocchio):
res = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
for limb in limbs:
effector = limbsCOMConstraints[limb]['effector']
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(config, limb)
#~ posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][effector] = {}
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][effector]["P"] = P
#~ res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += P
#~ res["contact_points"][effector]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["contact_points"][effector]["N"] = N
#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += N
return res
res = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
for limb in limbs:
effector = limbsCOMConstraints[limb]['effector']
#~ posetc = fullBody.getEffectorPosition(limb, config)
P, N = fullBody.computeContactForConfig(config, limb)
#~ posetc = fullBody.getEffectorPosition(limb, config)
res["contact_points"][effector] = {}
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["contact_points"][effector]["P"] = P
#~ res["P"] += [p for i, p in enumerate (posetc) if (i%2 == 0)]
res["P"] += P
#~ res["contact_points"][effector]["N"] = [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["contact_points"][effector]["N"] = N
#~ res["N"] += [n for i, n in enumerate (posetc) if (i%2 == 1)]
res["N"] += N
return res
from random import randint
def pos_quat_to_pinocchio(q):
q_res = q[:]
quat_end = q[4:7]
q_res[6] = q[3]
q_res[3:6] = quat_end
return q_res
def gen_contact_candidates_one_limb(limb, res, num_candidates = 10):
effectorName = limbsCOMConstraints[limb]['effector']
candidates = []
print "res", res
for i in range(num_candidates):
q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
fullBody.setCurrentConfig(q_contact)
m = _getTransform(fullBody.getJointPosition(effectorName))
candidates.append(m)
#DEBUG
res["config_candidates"].append(q_contact)
#~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
res[effectorName] = candidates
q_res = q[:]
quat_end = q[4:7]
q_res[6] = q[3]
q_res[3:6] = quat_end
return q_res
def gen_contact_candidates_one_limb(limb, data, num_candidates = 10):
effectorName = limbsCOMConstraints[limb]['effector']
candidates = []
config_candidates = [] #DEBUG
print "data", data
for i in range(num_candidates):
q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
fullBody.setCurrentConfig(q_contact)
m = _getTransform(fullBody.getJointPosition(effectorName))
candidates.append(m)
config_candidates.append(q_contact) #DEBUG
#~ candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
data[effectorName]["transforms"] = candidates
return config_candidates #DEBUG
def removeLimb(limb, limbs):
return [l for l in limbs if l != limb]
return [l for l in limbs if l != limb]
#~
#~ def find_limbs_broken(target_c, config, limbs):
#~ res = []
#~ for limb in limbs:
#~ nLimbs = removeLimb(limb, limbs)
#~ state_id = fullBody.createState(config, nLimbs)
#~ if (fullBody.projectStateToCOM(state_id,target_c)):
#~ res.append(limb)
#~ return res
#~ res = []
#~ for limb in limbs:
#~ nLimbs = removeLimb(limb, limbs)
#~ state_id = fullBody.createState(config, nLimbs)
#~ if (fullBody.projectStateToCOM(state_id,target_c)):
#~ res.append(limb)
#~ return res
def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
effector = limbsCOMConstraints[limb]['effector']
contact_points = {}
maintained_limbs = [l for l in limbs if limb != l]
for k, v in res["contact_points"].iteritems():
if k != effector:
contact_points[k] = v
success, dc, c_final, v0 = scv.comPosAfter07s(c, res["q"], contact_points)
effector_data = {}
state_id = fullBody.createState(config_gepetto, maintained_limbs)
if(success and fullBody.projectStateToCOM(state_id,c_final.tolist())): #all good, all contacts kinematically maintained):
effector_data["dc"] = dc
effector_data["c_final"] = c_final
proj_config = fullBody.getConfigAtState(state_id)
fullBody.setCurrentConfig(proj_config)
effector_data["projected_config"] = proj_config #DEBUG
data["candidates_per_effector"][effector] = effector_data
return True
return False
def gen_contact_candidates(limbs, config_gepetto, res):
#first generate a com translation current configuration
fullBody.setCurrentConfig(config_gepetto)
success, dc, c_final, v0 = scv.comPosAfter07s(matrix(fullBody.getCenterOfMass()), res["q"], res["contact_points"])
if(success):
data = {}
#set it as new com objective for projection
target_c = c_final.tolist()
state_id = fullBody.createState(config_gepetto, limbs)
#~ res["candidates"].append({})
data["v0"] = v0
data["dc"] = dc
data["c_final"] = c_final
data["candidateEffector"] = {}
#DEBUG
data["candidateEffector"]["config_candidates"] = []
if (fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
#~ res["candidates"][-1]["brokenContacts"] = find_limbs_broken(target_c, config_gepetto, limbs)
#~ for limb in limbs:
proj_config = fullBody.getConfigAtState(state_id)
#DEBUG
data["init_config"] = config_gepetto
#DEBUG
data["projected_config"] = proj_config
fullBody.setCurrentConfig(proj_config)
for limb in limbsCOMConstraints.keys():
gen_contact_candidates_one_limb(limb, data["candidateEffector"], 1)
if (not res.has_key("candidates")):
res["candidates"] = []
res["candidates"].append(data)
#first generate a com translation current configuration
fullBody.setCurrentConfig(config_gepetto)
c = matrix(fullBody.getCenterOfMass())
success, dc, v0 = scv.gen_com_vel(res["q"], res["contact_points"])
if(success):
data = {}
data["v0"] = v0
data["candidates_per_effector"] = {}
configs_candidates = [] #DEBUG
data["init_config"] = config_gepetto #DEBUG
for limb in limbsCOMConstraints.keys():
print "limb ", limb
if (predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto)): #all good, all contacts kinematically maintained
configs_candidates.append(gen_contact_candidates_one_limb(limb, data["candidates_per_effector"], 1)) #DEBUG
if(len(data["candidates_per_effector"].keys()) >0):
data["candidates_per_effector"]["config_candidates"] = configs_candidates #DEBUG
if (not res.has_key("candidates")):
res["candidates"] = []
res["candidates"].append(data)
from numpy import cos, sin, pi
def __eulerToQuat(pitch, roll, yaw):
t0 = cos(yaw * 0.5);
t1 = sin(yaw * 0.5);
t2 = cos(roll * 0.5);
t3 = sin(roll * 0.5);
t4 = cos(pitch * 0.5);
t5 = sin(pitch * 0.5);
w= t0 * t2 * t4 + t1 * t3 * t5;
x= t0 * t3 * t4 - t1 * t2 * t5;
y= t0 * t2 * t5 + t1 * t3 * t4;
z= t1 * t2 * t4 - t0 * t3 * t5;
return [w, x, y, z]
#~
t0 = cos(yaw * 0.5);
t1 = sin(yaw * 0.5);
t2 = cos(roll * 0.5);
t3 = sin(roll * 0.5);
t4 = cos(pitch * 0.5);
t5 = sin(pitch * 0.5);
w= t0 * t2 * t4 + t1 * t3 * t5;
x= t0 * t3 * t4 - t1 * t2 * t5;
y= t0 * t2 * t5 + t1 * t3 * t4;
z= t1 * t2 * t4 - t0 * t3 * t5;
return [w, x, y, z]
#~
#~ void SampleRotation(model::DevicePtr_t so3, ConfigurationPtr_t config, JointVector_t& jv)
#~ {
#~ std::size_t id = 1;
#~ if(so3->rootJoint())
#~ {
#~ Eigen::Matrix <value_type, 3, 1> confso3;
#~ id+=1;
#~ model::JointPtr_t joint = so3->rootJoint();
#~ for(int i =0; i <3; ++i)
#~ {
#~ joint->configuration()->uniformlySample (i, confso3);
#~ }
#~ Eigen::Quaterniond qt = Eigen::AngleAxisd(confso3(0), Eigen::Vector3d::UnitZ())
#~ * Eigen::AngleAxisd(confso3(1), Eigen::Vector3d::UnitY())
#~ * Eigen::AngleAxisd(confso3(2), Eigen::Vector3d::UnitX());
#~ std::size_t rank = 3;
#~ (*config)(rank+0) = qt.w();
#~ (*config)(rank+1) = qt.x();
#~ (*config)(rank+2) = qt.y();
#~ (*config)(rank+3) = qt.z();
#~ }
#~ if(id < jv.size())
#~ SampleRotationRec(config,jv,id);
#~ std::size_t id = 1;
#~ if(so3->rootJoint())
#~ {
#~ Eigen::Matrix <value_type, 3, 1> confso3;
#~ id+=1;
#~ model::JointPtr_t joint = so3->rootJoint();
#~ for(int i =0; i <3; ++i)
#~ {
#~ joint->configuration()->uniformlySample (i, confso3);
#~ }
#~ Eigen::Quaterniond qt = Eigen::AngleAxisd(confso3(0), Eigen::Vector3d::UnitZ())
#~ * Eigen::AngleAxisd(confso3(1), Eigen::Vector3d::UnitY())
#~ * Eigen::AngleAxisd(confso3(2), Eigen::Vector3d::UnitX());
#~ std::size_t rank = 3;
#~ (*config)(rank+0) = qt.w();
#~ (*config)(rank+1) = qt.x();
#~ (*config)(rank+2) = qt.y();
#~ (*config)(rank+3) = qt.z();
#~ }
#~ if(id < jv.size())
#~ SampleRotationRec(config,jv,id);
#~ }
from random import uniform
def _boundSO3(q, num_limbs):
q[:3] = [0,0,0.5]
limb_weight = float(4 - num_limbs)
#generate random angle
rot_y = uniform(-pi/(4+limb_weight), pi/(4+limb_weight))
rot_x = uniform(-pi/8, pi/(3+limb_weight))
rot_z = 0;
q[3:7] = __eulerToQuat(rot_x, rot_y, rot_z)
return q
q[:3] = [0,0,0.5]
limb_weight = float(4 - num_limbs)
#generate random angle
rot_y = uniform(-pi/(4+limb_weight), pi/(4+limb_weight))
rot_x = uniform(-pi/8, pi/(3+limb_weight))
rot_z = 0;
q[3:7] = __eulerToQuat(rot_x, rot_y, rot_z)
return q
def _genbalance(limbs):
for i in range(10000):
q = fullBody.client.basic.robot.shootRandomConfig()
q = _boundSO3(q, len(limbs))
if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return q
print "can't generate equilibrium config"
for i in range(10000):
q = fullBody.client.basic.robot.shootRandomConfig()
q = _boundSO3(q, len(limbs))
if fullBody.isConfigValid(q)[0] and fullBody.isConfigBalanced(q, limbs, 5) and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
#~ if fullBody.isConfigValid(q)[0] and __loosely_z_aligned(limbs[0], q) and __loosely_z_aligned(limbs[1], q):
return q
print "can't generate equilibrium config"
all_qs = []
all_states = []
def gen(limbs, num_samples = 1000, coplanar = True):
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = []
for _ in range(num_samples):
if(coplanar):
q = fullBody.generateGroundContact(limbs)
else:
q = _genbalance(limbs)
q_gep = q[:]
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
qs.append(q)
qs_gepetto.append(q_gep)
res = fill_contact_points(limbs,q_gep,q)
for _ in range(10):
gen_contact_candidates(limbs, q_gep, res)
states.append(res)
global all_qs
all_qs += [qs_gepetto]
fname = ""
for lname in limbs:
fname += lname + "_"
fname += "configs"
if(coplanar):
fname += "_coplanar"
from pickle import dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1=open(fname, 'w+')
dump(states, f1)
f1.close()
all_states.append(states)
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = []
for _ in range(num_samples):
if(coplanar):
q = fullBody.generateGroundContact(limbs)
else:
q = _genbalance(limbs)
q_gep = q[:]
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
res = fill_contact_points(limbs,q_gep,q)
for _ in range(1):
gen_contact_candidates(limbs, q_gep, res)
if(res.has_key("candidates")): #contact candidates found
states.append(res)
qs.append(q)
qs_gepetto.append(q_gep)
global all_qs
all_qs += [qs_gepetto]
fname = ""
for lname in limbs:
fname += lname + "_"
fname += "configs"
if(coplanar):
fname += "_coplanar"
from pickle import dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1=open(fname, 'w+')
dump(states, f1)
f1.close()
all_states.append(states)
j=0
q_init = [
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.1, -0.82, 0.648702, 1.0, 0.0 , 0.0, 0.0, # Free flyer 0-6
0.0, 0.0, 0.0, 0.0, # CHEST HEAD 7-10
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.261799388, 0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # LARM 11-17
0.261799388, -0.174532925, 0.0, -0.523598776, 0.0, 0.0, 0.17, # RARM 18-24
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # LLEG 25-30
0.0, 0.0, -0.453785606, 0.872664626, -0.41887902, 0.0, # RLEG 31-36
]; r (q_init)
......@@ -337,8 +350,39 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
#~ gen(limbs[0], 10)
#~ for ls in limbs:
#~ gen(ls, 10, False)
gen(limbs[0], 10)
#~ gen(ls, 10, False)
gen(limbs[0], 1)
i = 0
a = all_states[0][0]['candidates'][0]
b = a ['candidates_per_effector']
def init():
r(a['init_config'])
b = a ['candidates_per_effector']
def rleg():
r(b['RLEG_JOINT5']['projected_config'])
def lleg():
r(b['LLEG_JOINT5']['projected_config'])
def larm():
r(b['LARM_JOINT5']['projected_config'])
def rarm():
r(b['RARM_JOINT5']['projected_config'])
def c1():
r(b['config_candidates'][0][0])
def c2():
r(b['config_candidates'][1][0])
def c3():
r(b['config_candidates'][2][0])
def c4():
r(b['config_candidates'][3][0])
script/tools/tro_capture_point/sample_com_vel.py
View file @ 9dc2c37f
......@@ -110,7 +110,8 @@ def draw_q(q0):
def q_pin(q_hpp):
return np.matrix(q_hpp).T
def gen_com_vel(q0, contacts):
def gen_com_vel(q_hpp, contacts):
q0 = q_pin(q_hpp)
init(q0);
v0 = mat_zeros(nv);
invDynForm.setNewSensorData(0, q0, v0);
......@@ -127,12 +128,12 @@ def gen_com_vel(q0, contacts):
conf.MAX_INITIAL_COM_VEL, 100);
if success:
print "Initial velocities found"
return (success, v, invDynForm.J_com * v);
return (success, v[:], invDynForm.J_com * v);
print "Could not find initial velocities"
return (success, v[:]);
return (success, v[:], invDynForm.J_com * v);
def comPosAfter07s(c, q_hpp, contacts):
def comPosAfter07s(c, q_hpp, contacts, v = None):