Skip to content

GitLab

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

generation ok

parent ecab1d26
Hide whitespace changes
Inline Side-by-side
script/tools/tro_capture_point/gen_hrp2_statically_balanced_positions_2d.py
View file @ c5fc6077
......@@ -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.refresh()
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):
res = {}
res["q"] = config_pinocchio[:]
res["contact_points"] = {}
res["P"] = []
res["N"] = []
def fill_contact_points(limbs, fullbody, data):
data["contact_points"] = {}
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
P, N = fullBody.computeContactForConfig(fullbody.getCurrentConfig(), limb)
data["contact_points"][effector] = {}
data["contact_points"][effector]["P"] = P
data["contact_points"][effector]["N"] = N
return data
from random import randint
......@@ -166,7 +173,6 @@ 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)
......@@ -191,15 +197,19 @@ def removeLimb(limb, limbs):
#~ res.append(limb)
#~ 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']
contact_points = {}
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:
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 = {}
print "maintained_limbs ", 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):
effector_data["dc"] = dc
......@@ -209,14 +219,15 @@ def predict_com_for_limb_candidate(c, limb, limbs, res, data, config_gepetto):
effector_data["projected_config"] = proj_config #DEBUG
data["candidates_per_effector"][effector] = effector_data
return True
print "projection failed for limb ", limb
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
fullBody.setCurrentConfig(config_gepetto)
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):
data = {}
data["v0"] = v0
......@@ -225,9 +236,12 @@ def gen_contact_candidates(limbs, config_gepetto, res):
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):
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"], 10)) #DEBUG
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
if (not res.has_key("candidates")):
res["candidates"] = []
......@@ -301,7 +315,9 @@ all_states = []
def gen(limbs, num_samples = 1000, coplanar = True):
q_0 = fullBody.getCurrentConfig();
#~ fullBody.getSampleConfig()
qs = []; qs_gepetto = []; states = []
qs = []; qs_gepetto = []; states = []
data = {}
fill_contact_points(limbs, fullBody, data)
for _ in range(num_samples):
if(coplanar):
q = fullBody.generateGroundContact(limbs)
......@@ -311,9 +327,10 @@ def gen(limbs, num_samples = 1000, coplanar = True):
quat_end = q[4:7]
q[6] = q[3]
q[3:6] = quat_end
res = fill_contact_points(limbs,q_gep,q)
res = {}
res["q"] = q[:]
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
states.append(res)
qs.append(q)
......@@ -326,10 +343,11 @@ def gen(limbs, num_samples = 1000, coplanar = True):
fname += "configs"
if(coplanar):
fname += "_coplanar"
data["samples"] = states
from pickle import dump
#~ f1=open("configs_feet_on_ground_static_eq", 'w+')
f1=open(fname, 'w+')
dump(states, f1)
dump(data, f1)
f1.close()
all_states.append(states)
......@@ -348,10 +366,10 @@ limbs = [[lLegId,rLegId],[lLegId,rLegId, rarmId], [lLegId,rLegId, larmId], [lLeg
#~ limbs = [[lLegId,rLegId, rarmId]]
#~ limbs = [[larmId, rarmId]]
gen(limbs[0], 10)
for ls in limbs:
gen(ls, 10, False)
#~ gen(limbs[0], 10)
#~ for ls in limbs:
#~ gen(ls, 10, False)
gen(limbs[0], 1)
i = 0
......@@ -385,4 +403,12 @@ def c3():
def c4():
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']
script/tools/tro_capture_point/sample_com_vel.py
View file @ c5fc6077
......@@ -9,7 +9,7 @@ if cwd+'/data/config' not in sys.path:
sys.path += [cwd+'/data/config',];
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
import pinocchio as se3
......@@ -30,47 +30,47 @@ from numpy.linalg import norm
EPS = 1e-4;
def createInvDynFormUtil(q, v):
invDynForm = InvDynFormulation('inv_dyn', q, v, conf.dt, conf.model_path, conf.urdfFileName, conf.freeFlyer);
invDynForm.enableCapturePointLimits(conf.ENABLE_CAPTURE_POINT_LIMITS);
invDynForm.enableTorqueLimits(conf.ENABLE_TORQUE_LIMITS);
invDynForm.enableForceLimits(conf.ENABLE_FORCE_LIMITS);
invDynForm.enableJointLimits(conf.ENABLE_JOINT_LIMITS, conf.IMPOSE_POSITION_BOUNDS, conf.IMPOSE_VELOCITY_BOUNDS,
conf.IMPOSE_VIABILITY_BOUNDS, conf.IMPOSE_ACCELERATION_BOUNDS);
invDynForm.JOINT_POS_PREVIEW = conf.JOINT_POS_PREVIEW;
invDynForm.JOINT_VEL_PREVIEW = conf.JOINT_VEL_PREVIEW;
invDynForm.MAX_JOINT_ACC = conf.MAX_JOINT_ACC;
invDynForm.MAX_MIN_JOINT_ACC = conf.MAX_MIN_JOINT_ACC;
invDynForm.USE_JOINT_VELOCITY_ESTIMATOR = conf.USE_JOINT_VELOCITY_ESTIMATOR;
invDynForm.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
return invDynForm;
#~ def createInvDynFormUtil(q, v):
#~ invDynForm = InvDynFormulation('inv_dyn', q, v, conf.dt, conf.model_path, conf.urdfFileName, conf.freeFlyer);
#~ invDynForm.enableCapturePointLimits(conf.ENABLE_CAPTURE_POINT_LIMITS);
#~ invDynForm.enableTorqueLimits(conf.ENABLE_TORQUE_LIMITS);
#~ invDynForm.enableForceLimits(conf.ENABLE_FORCE_LIMITS);
#~ invDynForm.enableJointLimits(conf.ENABLE_JOINT_LIMITS, conf.IMPOSE_POSITION_BOUNDS, conf.IMPOSE_VELOCITY_BOUNDS,
#~ conf.IMPOSE_VIABILITY_BOUNDS, conf.IMPOSE_ACCELERATION_BOUNDS);
#~ invDynForm.JOINT_POS_PREVIEW = conf.JOINT_POS_PREVIEW;
#~ invDynForm.JOINT_VEL_PREVIEW = conf.JOINT_VEL_PREVIEW;
#~ invDynForm.MAX_JOINT_ACC = conf.MAX_JOINT_ACC;
#~ invDynForm.MAX_MIN_JOINT_ACC = conf.MAX_MIN_JOINT_ACC;
#~ invDynForm.USE_JOINT_VELOCITY_ESTIMATOR = conf.USE_JOINT_VELOCITY_ESTIMATOR;
#~ invDynForm.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
#~ return invDynForm;
def updateConstraints(t, q, v, invDynForm, contacts):
contact_changed = False;
for (name, PN) in contacts.iteritems():
if(invDynForm.existUnilateralContactConstraint(name)):
continue;
contact_changed =True;
invDynForm.r.forwardKinematics(q, v, 0 * v);
invDynForm.r.framesKinematics(q);
fid = invDynForm.getFrameId(name);
oMi = invDynForm.r.framePosition(fid);
ref_traj = ConstantSE3Trajectory(name, oMi);
constr = SE3Task(invDynForm.r, fid, ref_traj, name);
constr.kp = conf.kp_constr;
constr.kv = conf.kd_constr;
constr.mask(conf.constraint_mask);
Pi = np.matrix(PN['P']).T;
Ni = np.matrix(PN['N']).T;
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;
invDynForm.addUnilateralContactConstraint(constr, Pi, Ni, conf.fMin, conf.mu);
return contact_changed;
#~ def updateConstraints(t, q, v, invDynForm, contacts):
#~ contact_changed = False;
#~
#~ for (name, PN) in contacts.iteritems():
#~ if(invDynForm.existUnilateralContactConstraint(name)):
#~ continue;
#~
#~ contact_changed =True;
#~ invDynForm.r.forwardKinematics(q, v, 0 * v);
#~ invDynForm.r.framesKinematics(q);
#~
#~ fid = invDynForm.getFrameId(name);
#~ oMi = invDynForm.r.framePosition(fid);
#~ ref_traj = ConstantSE3Trajectory(name, oMi);
#~ constr = SE3Task(invDynForm.r, fid, ref_traj, name);
#~ constr.kp = conf.kp_constr;
#~ constr.kv = conf.kd_constr;
#~ constr.mask(conf.constraint_mask);
#~
#~ Pi = np.matrix(PN['P']).T;
#~ Ni = np.matrix(PN['N']).T;
#~ 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;
#~ invDynForm.addUnilateralContactConstraint(constr, Pi, Ni, conf.fMin, conf.mu);
#~
#~ return contact_changed;
def createSimulator(q0, v0):
......@@ -86,26 +86,13 @@ def createSimulator(q0, v0):
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
simulator.verb=0;
return simulator;
#~
def draw_q(q0):
p = np.matrix.copy(invDynForm.contact_points);
simulator.viewer.updateRobotConfig(q0);
simulator.updateComPositionInViewer(np.matrix([invDynForm.x_com[0,0], invDynForm.x_com[1,0], 0.]).T);
#~ p = np.matrix.copy(invDynForm.contact_points);
x_com = robot.com(q0)
simulator.updateComPositionInViewer(np.matrix([x_com[0,0], x_com[1,0], 0.]).T);
q0[2] -= 0.005
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):
return np.matrix(q_hpp).T
......@@ -114,41 +101,43 @@ def gen_com_vel(q_hpp, contacts):
q0 = q_pin(q_hpp)
init(q0);
v0 = mat_zeros(nv);
invDynForm.setNewSensorData(0, q0, v0);
updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0);
#~ invDynForm.setNewSensorData(0, q0, v0);
#~ updateConstraints(0, q0, v0, invDynForm, contacts);
print 'Gonna compute initial joint velocities that satisfy contact constraints';
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_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY,
True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL, 100);
True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL, conf.MAX_MIN_JOINT_ACC, 100);
#~ r(q_hpp)
draw_q(q0);
if success:
print "Initial velocities found"
return (success, v[:], invDynForm.J_com * v);
print "Initial velocities found"
J_com = robot.Jcom(q0, update_kinematics=False);
return (success, v[:], J_com * v);
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):
q0 = q_pin(q_hpp)
init(q0);
v0 = mat_zeros(nv);
invDynForm.setNewSensorData(0, q0, v0);
updateConstraints(0, q0, v0, invDynForm, contacts);
#~ invDynForm.setNewSensorData(0, q0, v0);
#~ updateConstraints(0, q0, v0, invDynForm, contacts);
#~ draw_q(q0);
print 'Gonna compute initial joint velocities that satisfy contact constraints';
print 'conf.MAX_INITIAL_COM_VEL', conf.MAX_INITIAL_COM_VEL
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_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY,
True, #conf.ENSURE_UNSTABILITY,
conf.MAX_INITIAL_COM_VEL, 100);
conf.MAX_INITIAL_COM_VEL,conf.MAX_MIN_JOINT_ACC , 100);
else:
success = True
if (not success):
......@@ -157,10 +146,19 @@ def com_pos_after_t(c, q_hpp, contacts, v = None, t = 0.7):
c_init = np.matrix(c)
dx_com_des = mat_zeros(3);
P = np.matlib.copy(invDynForm.contact_points);
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])
res = stab_criterion.predict_future_state(t, c_init, invDynForm.J_com * v)
#~ P = np.matlib.copy(invDynForm.contact_points);
#~ N = np.matlib.copy(invDynForm.contact_normals);
robot.computeAllTerms(q0, mat_zeros(nv));
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
print "c ", res.c
print "dc ", res.dc
......@@ -178,18 +176,16 @@ nv = robot.nv;
v0 = mat_zeros(nv);
invDynForm = None;
robot = None;
na = None; # number of joints
simulator = None;
def init(q0):
''' CREATE CONTROLLER AND SIMULATOR '''
global invDynForm
#~ global invDynForm
global robot
global na
global simulator
print "reset invdyn"
invDynForm = createInvDynFormUtil(q0, v0);
robot = invDynForm.r;
na = invDynForm.na; # number of joints
#~ invDynForm = createInvDynFormUtil(q0, v0);
robot = RobotWrapper( conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
simulator = createSimulator(q0, v0);
#~ gen_com_vel(q0, config_test['contact_points'])
script/tools/tro_capture_point/utils.py
View file @ c5fc6077
......@@ -12,28 +12,40 @@ from pinocchio.utils import zero as mat_zeros
from pinocchio.utils import rand as mat_rand
from pinocchio_inv_dyn.acc_bounds_util import computeVelLimits
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
class Bunch:
def __init__(self, **kwds):
self.__dict__.update(kwds);
def compute_contact_points_from_contact_dictionary(robot, contacts):
''' Compute the contact points and the contact normals in world reference frame
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):
ncp = np.sum([np.matrix(c['P']).shape[0] for c in contacts.itervalues()]);
p = mat_zeros((3,ncp)); # contact points in world frame
N = mat_zeros((3,ncp)); # contact normals in world frame
N[2,:] = 1.0;
(p, N) = compute_contact_points_from_contact_dictionary(robot, contacts);
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass, verb=verb);
c_pred = mat_zeros((3,len(contact_candidates)));
dc_pred = mat_zeros((3,len(contact_candidates)));
......@@ -41,27 +53,23 @@ def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts
t_pred = mat_zeros(len(contact_candidates));
# compute contact points for all contacts except the one to move
i = 0;
for (contact_name, PN) in contacts.iteritems():
if(contact_name==name_contact_to_break):
continue;
oMi = robot.framePosition(robot.model.getFrameId(contact_name));
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;
contacts_minus_one = dict(contacts);
del contacts_minus_one[name_contact_to_break];
(P_minus_one, N_minus_one) = compute_contact_points_from_contact_dictionary(robot, contacts_minus_one);
i = P_minus_one.shape[1];
p[:,:i] = P_minus_one;
N[:,:i] = N_minus_one;
c_id=0;
P_cand = np.matrix(contacts[name_contact_to_break]['P']).T; # contact points of contact to break in local frame
N_cand = np.matrix(contacts[name_contact_to_break]['N']).T; # contact normals of contact to break in local frame
for (c_id,oMi) in enumerate(contact_candidates):
# compute new position of contact points
for j in range(Pi.shape[1]):
for j in range(P_cand.shape[1]):
p[:,i+j] = oMi.act(P_cand[:,j]);
N[:,i+j] = oMi.rotation * N_cand[:,j];
if(viewer is not None):
if(verb>0 and viewer is not None):
# update contact points in viewer
for j in range(p.shape[1]):
viewer.addSphere("contact_point"+str(j), 0.005, p[:,j], (0.,0.,0.), (1, 1, 1, 1));
......@@ -93,16 +101,13 @@ def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts
dc=dc_pred[:,best_candidate_id], t=t_pred[best_candidate_id,0]);
def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, step_time):
def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, step_time, verb=0):
''' Select which contact to break
'''
ncp = np.sum([np.matrix(c['P']).shape[0] for c in contacts.itervalues()]);
# assume all contacts have the same number of contact points
ncp -= np.matrix(contacts.itervalues().next()['P']).shape[0];
p = mat_zeros((3,ncp));
N = mat_zeros((3,ncp));
p = mat_zeros((3,1));
N = mat_zeros((3,1));
N[2,:] = 1.0;
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass);
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass, verb=verb);
t_pred = mat_zeros(len(contacts));
t_min = mat_zeros(len(contacts));
v_pred = mat_zeros(len(contacts));
......@@ -111,18 +116,10 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
dc_min = mat_zeros((3,len(contacts)));
for (contactId, name_of_contact_to_break) in enumerate(contacts):
# compute the contact points without name_of_contact_to_break
i = 0;
for (contact_name, PN) in contacts.iteritems():
if(contact_name==name_of_contact_to_break):
continue;
fid = robot.model.getFrameId(contact_name);
oMi = robot.framePosition(fid);
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;
contacts_minus_one = dict(contacts);
del contacts_minus_one[name_of_contact_to_break];
(p,N) = compute_contact_points_from_contact_dictionary(robot, contacts_minus_one);
# predict future com state supposing to break name_of_contact_to_break
stabilityCriterion.set_contacts(p.T, N.T, mu);
try:
......@@ -133,8 +130,9 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
dc_pred[:,contactId] = np.asmatrix(res.dc).T;
dc_min[:,contactId] = np.asmatrix(res.dc_min).T;
v_pred[contactId] = norm(res.dc);
# print "Without contact %s robot will be able to maintain the contact for %.3f s"%(name_of_contact_to_break,t);
# print " Predicted com state = (", c_t.T, dc_t.T, "), norm(dc)=%.3f"%norm(dc_t);
if(verb>0):
print "[select_contact_to_break] Without contact %s contacts can be kept for %.3f s"%(name_of_contact_to_break,res.t);
print " Predicted com state = (", res.c.T, res.dc.T, "), norm(dc)=%.3f"%norm(res.dc);
except Exception as e:
print "ERROR while computing stability criterion:", e;
......@@ -147,6 +145,22 @@ def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, s
return Bunch(name=name_of_contact_to_break, c=c_pred[:,id_contact_to_break],
dc=dc_pred[:,id_contact_to_break], t=t_pred[id_contact_to_break,0],
t_min=t_min[id_contact_to_break,0], dc_min=dc_min[:,id_contact_to_break]);
def predict_future_com_state(x_com, dx_com, robot, mass, contacts, mu, gravity, time, verb=0):
''' Predict future com state
'''
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts);
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, P.T, N.T, mu, gravity, mass, verb=verb);