Commit c5fc6077 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

generation ok

parent ecab1d26
...@@ -128,29 +128,36 @@ def draw_cp(cid, limb, config): ...@@ -128,29 +128,36 @@ def draw_cp(cid, limb, config):
r.client.gui.applyConfiguration(scene+"/b"+str(i),pos.tolist()+[1,0,0,0]) r.client.gui.applyConfiguration(scene+"/b"+str(i),pos.tolist()+[1,0,0,0])
r.client.gui.refresh() r.client.gui.refresh()
r.client.gui.addSceneToWindow(scene,0) r.client.gui.addSceneToWindow(scene,0)
def draw_com(config):
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): def fill_contact_points(limbs, fullbody, data):
res = {} data["contact_points"] = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
for limb in limbs: for limb in limbs:
effector = limbsCOMConstraints[limb]['effector'] effector = limbsCOMConstraints[limb]['effector']
#~ posetc = fullBody.getEffectorPosition(limb, config) P, N = fullBody.computeContactForConfig(fullbody.getCurrentConfig(), limb)
P, N = fullBody.computeContactForConfig(config, limb) data["contact_points"][effector] = {}
#~ posetc = fullBody.getEffectorPosition(limb, config) data["contact_points"][effector]["P"] = P
res["contact_points"][effector] = {} data["contact_points"][effector]["N"] = N
#~ res["contact_points"][effector]["P"] = [p for i, p in enumerate (posetc) if (i%2 == 0)] return data
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 from random import randint
...@@ -166,7 +173,6 @@ def gen_contact_candidates_one_limb(limb, data, num_candidates = 10): ...@@ -166,7 +173,6 @@ def gen_contact_candidates_one_limb(limb, data, num_candidates = 10):
effectorName = limbsCOMConstraints[limb]['effector'] effectorName = limbsCOMConstraints[limb]['effector']
candidates = [] candidates = []
config_candidates = [] #DEBUG config_candidates = [] #DEBUG
print "data", data
for i in range(num_candidates): for i in range(num_candidates):
q_contact = fullBody.getSample(limb,randint(0,n_samples - 1)) q_contact = fullBody.getSample(limb,randint(0,n_samples - 1))
fullBody.setCurrentConfig(q_contact) fullBody.setCurrentConfig(q_contact)
...@@ -191,15 +197,19 @@ def removeLimb(limb, limbs): ...@@ -191,15 +197,19 @@ def removeLimb(limb, limbs):
#~ res.append(limb) #~ res.append(limb)
#~ return res #~ return res
def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto): def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto, orig_contact_points):
effector = limbsCOMConstraints[limb]['effector'] effector = limbsCOMConstraints[limb]['effector']
contact_points = {} contact_points = {}
maintained_limbs = [l for l in limbs if limb != l] maintained_limbs = [l for l in limbs if limb != l]
for k, v in res["contact_points"].iteritems(): for k, v in orig_contact_points.iteritems():
if k != effector: if k != effector:
contact_points[k] = v contact_points[k] = v
success, dc, c_final, v0 = scv.com_pos_after_t(c, res["q"], contact_points) #~ success, dc, c_final, v0 = scv.com_pos_after_t(c, res["q"], contact_points, data["v0"])
success, dc, c_final, v0 = scv.com_pos_after_t(c, res["q"], contact_points, data["v0"])
print "c for limb ", c_final , ":", limb
print "success ",success
effector_data = {} effector_data = {}
print "maintained_limbs ", maintained_limbs
state_id = fullBody.createState(config_gepetto, maintained_limbs) state_id = fullBody.createState(config_gepetto, maintained_limbs)
if(success and fullBody.projectStateToCOM(state_id,c_final.tolist())): #all good, all contacts kinematically maintained): if(success and fullBody.projectStateToCOM(state_id,c_final.tolist())): #all good, all contacts kinematically maintained):
effector_data["dc"] = dc effector_data["dc"] = dc
...@@ -209,14 +219,15 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto): ...@@ -209,14 +219,15 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
effector_data["projected_config"] = proj_config #DEBUG effector_data["projected_config"] = proj_config #DEBUG
data["candidates_per_effector"][effector] = effector_data data["candidates_per_effector"][effector] = effector_data
return True return True
print "projection failed for limb ", limb
return False return False
def gen_contact_candidates(limbs, config_gepetto, res): def gen_contact_candidates(limbs, config_gepetto, res, contact_points):
#first generate a com translation current configuration #first generate a com translation current configuration
fullBody.setCurrentConfig(config_gepetto) fullBody.setCurrentConfig(config_gepetto)
c = matrix(fullBody.getCenterOfMass()) c = matrix(fullBody.getCenterOfMass())
success, dc, v0 = scv.gen_com_vel(res["q"], res["contact_points"]) success, v0, dc = scv.gen_com_vel(res["q"], contact_points)
if(success): if(success):
data = {} data = {}
data["v0"] = v0 data["v0"] = v0
...@@ -225,9 +236,12 @@ def gen_contact_candidates(limbs, config_gepetto, res): ...@@ -225,9 +236,12 @@ def gen_contact_candidates(limbs, config_gepetto, res):
data["init_config"] = config_gepetto #DEBUG data["init_config"] = config_gepetto #DEBUG
for limb in limbsCOMConstraints.keys(): for limb in limbsCOMConstraints.keys():
print "limb ", limb print "limb ", limb
if (predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto)): #all good, all contacts kinematically maintained if (predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto, contact_points)): #all good, all contacts kinematically maintained
configs_candidates.append(gen_contact_candidates_one_limb(limb, data["candidates_per_effector"], 1)) #DEBUG configs_candidates.append(gen_contact_candidates_one_limb(limb, data["candidates_per_effector"], 10)) #DEBUG
if(len(data["candidates_per_effector"].keys()) >0): if(len(data["candidates_per_effector"].keys()) >0):
#~ if((data["candidates_per_effector"].has_key('RARM_JOINT5') and not data["candidates_per_effector"].has_key('LARM_JOINT5')) or
#~ (data["candidates_per_effector"].has_key('LARM_JOINT5') and not data["candidates_per_effector"].has_key('RARM_JOINT5'))):
#~ raise ValueError("RARM AND LARM candidates not coherent (if there is candidates for one there should be for the other)");
data["candidates_per_effector"]["config_candidates"] = configs_candidates #DEBUG data["candidates_per_effector"]["config_candidates"] = configs_candidates #DEBUG
if (not res.has_key("candidates")): if (not res.has_key("candidates")):
res["candidates"] = [] res["candidates"] = []
...@@ -301,7 +315,9 @@ all_states = [] ...@@ -301,7 +315,9 @@ all_states = []
def gen(limbs, num_samples = 1000, coplanar = True): def gen(limbs, num_samples = 1000, coplanar = True):
q_0 = fullBody.getCurrentConfig(); q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig() #~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = [] qs = []; qs_gepetto = []; states = []
data = {}
fill_contact_points(limbs, fullBody, data)
for _ in range(num_samples): for _ in range(num_samples):
if(coplanar): if(coplanar):
q = fullBody.generateGroundContact(limbs) q = fullBody.generateGroundContact(limbs)
...@@ -311,9 +327,10 @@ def gen(limbs, num_samples = 1000, coplanar = True): ...@@ -311,9 +327,10 @@ def gen(limbs, num_samples = 1000, coplanar = True):
quat_end = q[4:7] quat_end = q[4:7]
q[6] = q[3] q[6] = q[3]
q[3:6] = quat_end q[3:6] = quat_end
res = fill_contact_points(limbs,q_gep,q) res = {}
res["q"] = q[:]
for _ in range(1): for _ in range(1):
gen_contact_candidates(limbs, q_gep, res) gen_contact_candidates(limbs, q_gep, res, data["contact_points"])
if(res.has_key("candidates")): #contact candidates found if(res.has_key("candidates")): #contact candidates found
states.append(res) states.append(res)
qs.append(q) qs.append(q)
...@@ -326,10 +343,11 @@ def gen(limbs, num_samples = 1000, coplanar = True): ...@@ -326,10 +343,11 @@ def gen(limbs, num_samples = 1000, coplanar = True):
fname += "configs" fname += "configs"
if(coplanar): if(coplanar):
fname += "_coplanar" fname += "_coplanar"
data["samples"] = states
from pickle import dump from pickle import dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+') #~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1=open(fname, 'w+') f1=open(fname, 'w+')
dump(states, f1) dump(data, f1)
f1.close() f1.close()
all_states.append(states) all_states.append(states)
...@@ -348,10 +366,10 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg ...@@ -348,10 +366,10 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
#~ limbs = [[lLegId,rLegId, rarmId]] #~ limbs = [[lLegId,rLegId, rarmId]]
#~ limbs = [[larmId, rarmId]] #~ limbs = [[larmId, rarmId]]
gen(limbs[0], 10)
for ls in limbs:
gen(ls, 10, False)
#~ gen(limbs[0], 10) #~ gen(limbs[0], 10)
#~ for ls in limbs:
#~ gen(ls, 10, False)
gen(limbs[0], 1)
i = 0 i = 0
...@@ -385,4 +403,12 @@ def c3(): ...@@ -385,4 +403,12 @@ def c3():
def c4(): def c4():
r(b['config_candidates'][3][0]) r(b['config_candidates'][3][0])
def inc():
global i
global a
global b
i+=1
a = all_states[0][i]['candidates'][0]
b = a ['candidates_per_effector']
...@@ -9,7 +9,7 @@ if cwd+'/data/config' not in sys.path: ...@@ -9,7 +9,7 @@ if cwd+'/data/config' not in sys.path:
sys.path += [cwd+'/data/config',]; sys.path += [cwd+'/data/config',];
import conf_hpp as conf import conf_hpp as conf
from utils import compute_initial_joint_velocities_multi_contact from utils import compute_initial_joint_velocities_multi_contact, compute_contact_points_from_contact_dictionary
from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion
import pinocchio as se3 import pinocchio as se3
...@@ -30,47 +30,47 @@ from numpy.linalg import norm ...@@ -30,47 +30,47 @@ from numpy.linalg import norm
EPS = 1e-4; EPS = 1e-4;
def createInvDynFormUtil(q, v): #~ def createInvDynFormUtil(q, v):
invDynForm = InvDynFormulation('inv_dyn', q, v, conf.dt, conf.model_path, conf.urdfFileName, conf.freeFlyer); #~ invDynForm = InvDynFormulation('inv_dyn', q, v, conf.dt, conf.model_path, conf.urdfFileName, conf.freeFlyer);
invDynForm.enableCapturePointLimits(conf.ENABLE_CAPTURE_POINT_LIMITS); #~ invDynForm.enableCapturePointLimits(conf.ENABLE_CAPTURE_POINT_LIMITS);
invDynForm.enableTorqueLimits(conf.ENABLE_TORQUE_LIMITS); #~ invDynForm.enableTorqueLimits(conf.ENABLE_TORQUE_LIMITS);
invDynForm.enableForceLimits(conf.ENABLE_FORCE_LIMITS); #~ invDynForm.enableForceLimits(conf.ENABLE_FORCE_LIMITS);
invDynForm.enableJointLimits(conf.ENABLE_JOINT_LIMITS, conf.IMPOSE_POSITION_BOUNDS, conf.IMPOSE_VELOCITY_BOUNDS, #~ invDynForm.enableJointLimits(conf.ENABLE_JOINT_LIMITS, conf.IMPOSE_POSITION_BOUNDS, conf.IMPOSE_VELOCITY_BOUNDS,
conf.IMPOSE_VIABILITY_BOUNDS, conf.IMPOSE_ACCELERATION_BOUNDS); #~ conf.IMPOSE_VIABILITY_BOUNDS, conf.IMPOSE_ACCELERATION_BOUNDS);
invDynForm.JOINT_POS_PREVIEW = conf.JOINT_POS_PREVIEW; #~ invDynForm.JOINT_POS_PREVIEW = conf.JOINT_POS_PREVIEW;
invDynForm.JOINT_VEL_PREVIEW = conf.JOINT_VEL_PREVIEW; #~ invDynForm.JOINT_VEL_PREVIEW = conf.JOINT_VEL_PREVIEW;
invDynForm.MAX_JOINT_ACC = conf.MAX_JOINT_ACC; #~ invDynForm.MAX_JOINT_ACC = conf.MAX_JOINT_ACC;
invDynForm.MAX_MIN_JOINT_ACC = conf.MAX_MIN_JOINT_ACC; #~ invDynForm.MAX_MIN_JOINT_ACC = conf.MAX_MIN_JOINT_ACC;
invDynForm.USE_JOINT_VELOCITY_ESTIMATOR = conf.USE_JOINT_VELOCITY_ESTIMATOR; #~ invDynForm.USE_JOINT_VELOCITY_ESTIMATOR = conf.USE_JOINT_VELOCITY_ESTIMATOR;
invDynForm.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS; #~ invDynForm.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
return invDynForm; #~ return invDynForm;
def updateConstraints(t, q, v, invDynForm, contacts): #~ def updateConstraints(t, q, v, invDynForm, contacts):
contact_changed = False; #~ contact_changed = False;
#~
for (name, PN) in contacts.iteritems(): #~ for (name, PN) in contacts.iteritems():
if(invDynForm.existUnilateralContactConstraint(name)): #~ if(invDynForm.existUnilateralContactConstraint(name)):
continue; #~ continue;
#~
contact_changed =True; #~ contact_changed =True;
invDynForm.r.forwardKinematics(q, v, 0 * v); #~ invDynForm.r.forwardKinematics(q, v, 0 * v);
invDynForm.r.framesKinematics(q); #~ invDynForm.r.framesKinematics(q);
#~
fid = invDynForm.getFrameId(name); #~ fid = invDynForm.getFrameId(name);
oMi = invDynForm.r.framePosition(fid); #~ oMi = invDynForm.r.framePosition(fid);
ref_traj = ConstantSE3Trajectory(name, oMi); #~ ref_traj = ConstantSE3Trajectory(name, oMi);
constr = SE3Task(invDynForm.r, fid, ref_traj, name); #~ constr = SE3Task(invDynForm.r, fid, ref_traj, name);
constr.kp = conf.kp_constr; #~ constr.kp = conf.kp_constr;
constr.kv = conf.kd_constr; #~ constr.kv = conf.kd_constr;
constr.mask(conf.constraint_mask); #~ constr.mask(conf.constraint_mask);
#~
Pi = np.matrix(PN['P']).T; #~ Pi = np.matrix(PN['P']).T;
Ni = np.matrix(PN['N']).T; #~ Ni = np.matrix(PN['N']).T;
for j in range(Pi.shape[1]): #~ for j in range(Pi.shape[1]):
print " contact point %d in world frame:"%j, oMi.act(Pi[:,j]).T, (oMi.rotation * Ni[:,j]).T; #~ print " contact point %d in world frame:"%j, oMi.act(Pi[:,j]).T, (oMi.rotation * Ni[:,j]).T;
invDynForm.addUnilateralContactConstraint(constr, Pi, Ni, conf.fMin, conf.mu); #~ invDynForm.addUnilateralContactConstraint(constr, Pi, Ni, conf.fMin, conf.mu);
#~
return contact_changed; #~ return contact_changed;
def createSimulator(q0, v0): def createSimulator(q0, v0):
...@@ -86,26 +86,13 @@ def createSimulator(q0, v0): ...@@ -86,26 +86,13 @@ def createSimulator(q0, v0):
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3; simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
simulator.verb=0; simulator.verb=0;
return simulator; return simulator;
#~
def draw_q(q0): def draw_q(q0):
p = np.matrix.copy(invDynForm.contact_points); #~ p = np.matrix.copy(invDynForm.contact_points);
simulator.viewer.updateRobotConfig(q0); x_com = robot.com(q0)
simulator.updateComPositionInViewer(np.matrix([invDynForm.x_com[0,0], invDynForm.x_com[1,0], 0.]).T); simulator.updateComPositionInViewer(np.matrix([x_com[0,0], x_com[1,0], 0.]).T);
q0[2] -= 0.005 q0[2] -= 0.005
simulator.viewer.updateRobotConfig(q0); simulator.viewer.updateRobotConfig(q0);
p[2,:] -= 0.005;
for j in range(p.shape[1]):
simulator.viewer.addSphere("contact_point"+str(j), 0.005, p[:,j], (0.,0.,0.), (1, 1, 1, 1));
simulator.viewer.updateObjectConfigRpy("contact_point"+str(j), p[:,j]);
f = open(conf.INITIAL_CONFIG_FILENAME, 'rb');
res = pickle.load(f);
f.close();
p_steve = np.matrix(res[conf.INITIAL_CONFIG_ID]['P']);
p_steve[:,2] -= 0.005;
for j in range(p_steve.shape[0]):
simulator.viewer.addSphere("contact_point_steve"+str(j), 0.005, p_steve[j,:].T, color=(1, 0, 0, 1));
simulator.viewer.updateObjectConfigRpy("contact_point_steve"+str(j), p_steve[j,:].T);
def q_pin(q_hpp): def q_pin(q_hpp):
return np.matrix(q_hpp).T return np.matrix(q_hpp).T
...@@ -114,41 +101,43 @@ def gen_com_vel(q_hpp, contacts): ...@@ -114,41 +101,43 @@ def gen_com_vel(q_hpp, contacts):
q0 = q_pin(q_hpp) q0 = q_pin(q_hpp)
init(q0); init(q0);
v0 = mat_zeros(nv); v0 = mat_zeros(nv);
invDynForm.setNewSensorData(0, q0, v0); #~ invDynForm.setNewSensorData(0, q0, v0);
updateConstraints(0, q0, v0, invDynForm, contacts); #~ updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0);
print 'Gonna compute initial joint velocities that satisfy contact constraints'; print 'Gonna compute initial joint velocities that satisfy contact constraints';
print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL
(success, v)= compute_initial_joint_velocities_multi_contact(q0, invDynForm, conf.mu[0], (success, v)= compute_initial_joint_velocities_multi_contact(q0, robot, contacts, np.array([True]*6), conf.mu[0],
conf.ZERO_INITIAL_ANGULAR_MOMENTUM, conf.ZERO_INITIAL_ANGULAR_MOMENTUM,
conf.ZERO_INITIAL_VERTICAL_COM_VEL, conf.ZERO_INITIAL_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY, False, #conf.ENSURE_STABILITY,
True, #conf.ENSURE_UNSTABILITY, True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL, 100); conf.MAX_INITIAL_COM_VEL, conf.MAX_MIN_JOINT_ACC, 100);
#~ r(q_hpp)
draw_q(q0);
if success: if success:
print "Initial velocities found" print "Initial velocities found"
return (success, v[:], invDynForm.J_com * v); J_com = robot.Jcom(q0, update_kinematics=False);
return (success, v[:], J_com * v);
print "Could not find initial velocities" print "Could not find initial velocities"
return (success, v[:], invDynForm.J_com * v); return (success, v[:], v);
def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7): def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
q0 = q_pin(q_hpp) q0 = q_pin(q_hpp)
init(q0); init(q0);
v0 = mat_zeros(nv); v0 = mat_zeros(nv);
invDynForm.setNewSensorData(0, q0, v0); #~ invDynForm.setNewSensorData(0, q0, v0);
updateConstraints(0, q0, v0, invDynForm, contacts); #~ updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0); #~ draw_q(q0);
print 'Gonna compute initial joint velocities that satisfy contact constraints'; print 'Gonna compute initial joint velocities that satisfy contact constraints';
print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL
if(v == None): if(v == None):
(success, v) = compute_initial_joint_velocities_multi_contact(q0, invDynForm, conf.mu[0], (success, v) = compute_initial_joint_velocities_multi_contact(q0, robot, contacts, [True]*6, conf.mu[0],
conf.ZERO_INITIAL_ANGULAR_MOMENTUM, conf.ZERO_INITIAL_ANGULAR_MOMENTUM,
conf.ZERO_INITIAL_VERTICAL_COM_VEL, conf.ZERO_INITIAL_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY, False, #conf.ENSURE_STABILITY,
True, #conf.ENSURE_UNSTABILITY, True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL, 100); conf.MAX_INITIAL_COM_VEL,conf.MAX_MIN_JOINT_ACC , 100);
else: else:
success = True success = True
if (not success): if (not success):
...@@ -157,10 +146,19 @@ def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7): ...@@ -157,10 +146,19 @@ def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
c_init = np.matrix(c) c_init = np.matrix(c)
dx_com_des = mat_zeros(3); dx_com_des = mat_zeros(3);
P = np.matlib.copy(invDynForm.contact_points); #~ P = np.matlib.copy(invDynForm.contact_points);
N = np.matlib.copy(invDynForm.contact_normals); #~ N = np.matlib.copy(invDynForm.contact_normals);
stab_criterion = StabilityCriterion("default", invDynForm.x_com, dx_com_des, P.T, N.T, conf.mu[0], np.array([0,0,-9.81]), invDynForm.M[0,0]) robot.computeAllTerms(q0, mat_zeros(nv));
res = stab_criterion.predict_future_state(t, c_init, invDynForm.J_com * v) robot.framesKinematics(q0);
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts)
print "contacts ", len(contacts)
print "contacts ", contacts
print "normals ", N
M = robot.mass(q0, update_kinematics=True);
J_com = robot.Jcom(q0, update_kinematics=False);
print "Mas ", M
stab_criterion = StabilityCriterion("default", np.matrix(c), dx_com_des, P.T, N.T, conf.mu[0], np.array([0,0,-9.81]), M[0,0])
res = stab_criterion.predict_future_state(t, c_init, J_com * v)
#TODO : res.t != 0.7 #TODO : res.t != 0.7
print "c ", res.c print "c ", res.c
print "dc ", res.dc print "dc ", res.dc
...@@ -178,18 +176,16 @@ nv = robot.nv; ...@@ -178,18 +176,16 @@ nv = robot.nv;
v0 = mat_zeros(nv); v0 = mat_zeros(nv);
invDynForm = None; invDynForm = None;
robot = None; robot = None;
na = None; # number of joints
simulator = None; simulator = None;
def init(q0): def init(q0):
''' CREATE CONTROLLER AND SIMULATOR ''' ''' CREATE CONTROLLER AND SIMULATOR '''
global invDynForm #~ global invDynForm
global robot global robot
global na global na
global simulator global simulator
print "reset invdyn" print "reset invdyn"
invDynForm = createInvDynFormUtil(q0, v0); #~ invDynForm = createInvDynFormUtil(q0, v0);
robot = invDynForm.r; robot = RobotWrapper( conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
na = invDynForm.na; # number of joints
simulator = createSimulator(q0, v0); simulator = createSimulator(q0, v0);
#~ gen_com_vel(q0, config_test['contact_points']) #~ gen_com_vel(q0, config_test['contact_points'])
...@@ -12,28 +12,40 @@ from pinocchio.utils import zero as mat_zeros ...@@ -12,28 +12,40 @@ from pinocchio.utils import zero as mat_zeros
from pinocchio.utils import rand as mat_rand from pinocchio.utils import rand as mat_rand
from pinocchio_inv_dyn.acc_bounds_util import computeVelLimits from pinocchio_inv_dyn.acc_bounds_util import computeVelLimits
from pinocchio_inv_dyn.sot_utils import solveLeastSquare from pinocchio_inv_dyn.sot_utils import solveLeastSquare
from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion, Bunch
EPS = 1e-5 EPS = 1e-5
class Bunch: def compute_contact_points_from_contact_dictionary(robot, contacts):
def __init__(self, **kwds): ''' Compute the contact points and the contact normals in world reference frame
self.__dict__.update(kwds); starting from a dictionary of contact points and contact normals in local frame.
robot -- An instance of RobotWrapper
contacts -- A dictionary containing the contact points and normals in local frame
and using the joint names as keys
Output a tuple (P, N) where:
P -- A 3xk numpy matrix containing all the 3d contact points in world frame
N -- A 3xk numpy matrix containing all the 3d contact normals in world frame
'''
ncp = np.sum([np.matrix(c['P']).shape[0] for c in contacts.itervalues()]);
P = mat_zeros((3,ncp));
N = mat_zeros((3,ncp));
i = 0;
for (contact_name, PN) in contacts.iteritems():
print "key ", contact_name, robot.model.getFrameId(contact_name)
print "existe ", contact_name, robot.model.existFrame(contact_name)
oMi = robot.framePosition(robot.model.getFrameId(contact_name));
print "oMi ", oMi
Pi = np.matrix(PN['P']).T;
Ni = np.matrix(PN['N']).T;
for j in range(Pi.shape[1]):
P[:,i] = oMi.act(Pi[:,j]);
N[:,i] = oMi.rotation * Ni[:,j];
i += 1;
return (P, N);
def __str__(self):
res = "";
for (key,value) in self.__dict__.iteritems():
if (isinstance(value, np.ndarray) and len(value.shape)==2 and value.shape[0]>value.shape[1]):
res += " - " + key + ": " + str(value.T) + "\n";
else:
res += " - " + key + ": " + str(value) + "\n";
return res[:-1];
def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts, contact_candidates, mu, gravity, mass, viewer=None, verb=0):