Commit 4022d15f authored by Steve Tonneau's avatar Steve Tonneau
Browse files

functional version of candidate sampling. must make call to...

functional version of candidate sampling. must make call to predict_future_state for each broken contact scenario
parent 011b4a8e
......@@ -162,33 +162,66 @@ def pos_quat_to_pinocchio(q):
q_res[3:6] = quat_end
return q_res
def gen_contact_candidates_one_limb(limb, config_gepetto, res, num_candidates = 10):
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):
fullBody.setCurrentConfig(fullBody.getSample(limb,randint(0,n_samples - 1)))
#~ m = _getTransform(fullBody.getJointPosition(effectorName))
candidates.append(pos_quat_to_pinocchio(fullBody.getJointPosition(effectorName)))
res["candidates"][effectorName] = 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
def find_limbs_broken(target_c, config, limbs):
def removeLimb(limb, limbs):
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
def gen_contact_candidates(limbs, config_gepetto, res):
#first generate a com translation current configuration
success, dc, delta_c = scv.comTranslationAfter07s(res["q"], res["contact_points"])
#set it as new com objective for projection
fullBody.setCurrentConfig(config_gepetto)
c = array(fullBody.getCenterOfMass())
target_c = c + delta_c
state_id = fullBody.createState(config, limbs)
res["candidates"] = {}
brokenContacts = []
candidateLimbs = []
if(fullBody.projectStateToCOM(state_id,target_c)): #all good, all contacts kinematically maintained
for limb in limbs:
gen_contact_candidates_one_limb(limb, config_gepetto, res, 10)
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)
from numpy import cos, sin, pi
def __eulerToQuat(pitch, roll, yaw):
......@@ -268,7 +301,10 @@ def gen(limbs, num_samples = 1000, coplanar = True):
q[3:6] = quat_end
qs.append(q)
qs_gepetto.append(q_gep)
states.append(fill_contact_points(limbs,q_gep,q))
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 = ""
......
......@@ -10,6 +10,7 @@ if cwd+'/data/config' not in sys.path:
import conf_hpp as conf
from utils import compute_initial_joint_velocities_multi_contact
from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion
import pinocchio as se3
from pinocchio.utils import zero as mat_zeros
......@@ -86,28 +87,28 @@ def createSimulator(q0, v0):
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);
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 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);
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
return np.matrix(q_hpp).T
def gen_com_vel(q0, contacts):
init(q0);
......@@ -119,29 +120,54 @@ def gen_com_vel(q0, 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],
conf.ZERO_INITIAL_ANGULAR_MOMENTUM,
conf.ZERO_INITIAL_VERTICAL_COM_VEL,
False, #conf.ENSURE_STABILITY,
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);
if success:
if success:
print "Initial velocities found"
return (success, invDynForm.J_com * v);
return (success, v, invDynForm.J_com * v);
print "Could not find initial velocities"
return (success, v[:]);
def comTranslationAfter07s(q_hpp, contacts):
(success, dc0)= gen_com_vel(q_pin(q_hpp), contacts)
return success, dc0, dc0 * 0.7
def comPosAfter07s(c, 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);
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],
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);
if (not success):
print "Could not find initial velocities"
return (success, v[:], c, v0);
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(0.7, c_init, invDynForm.J_com * v)
print "c ", res.c
print "dc ", res.dc
return success, res.dc, res.c, v0
np.set_printoptions(precision=2, suppress=True);
if(conf.freeFlyer):
robot = RobotWrapper(conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
robot = RobotWrapper(conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
else:
robot = RobotWrapper(conf.urdfFileName, conf.model_path, None);
robot = RobotWrapper(conf.urdfFileName, conf.model_path, None);
nq = robot.nq;
nv = robot.nv;
v0 = mat_zeros(nv);
......@@ -151,14 +177,14 @@ na = None; # number of joints
simulator = None;
def init(q0):
''' CREATE CONTROLLER AND SIMULATOR '''
global invDynForm
global robot
global na
global simulator
print "reset invdyn"
invDynForm = createInvDynFormUtil(q0, v0);
robot = invDynForm.r;
na = invDynForm.na; # number of joints
simulator = createSimulator(q0, v0);
#~ gen_com_vel(q0, config_test['contact_points'])
''' CREATE CONTROLLER AND SIMULATOR '''
global invDynForm
global robot
global na
global simulator
print "reset invdyn"
invDynForm = createInvDynFormUtil(q0, v0);
robot = invDynForm.r;
na = invDynForm.na; # number of joints
simulator = createSimulator(q0, v0);
#~ gen_com_vel(q0, config_test['contact_points'])
......@@ -12,9 +12,141 @@ 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.utils import can_I_stop
from pinocchio_inv_dyn.multi_contact.stability_criterion import StabilityCriterion
EPS = 1e-5
class Bunch:
def __init__(self, **kwds):
self.__dict__.update(kwds);
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;
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)));
v_pred = mat_zeros(len(contact_candidates));
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;
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]):
p[:,i+j] = oMi.act(P_cand[:,j]);
N[:,i+j] = oMi.rotation * N_cand[:,j];
if(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));
viewer.updateObjectConfigRpy("contact_point"+str(j), p[:,j]);
# check whether the system can stop with this contacts
stabilityCriterion.set_contacts(p.T, N.T, mu);
try:
stab_res = stabilityCriterion.can_I_stop(x_com, dx_com);
c_pred[:,c_id] = np.asmatrix(stab_res.c).T;
dc_pred[:,c_id] = np.asmatrix(stab_res.dc).T;
v_pred[c_id] = norm(stab_res.dc);
t_pred[c_id] = stab_res.t;
if(verb>0):
print "[select_contact_to_make] Candidate %d, predicted com vel in %.3f s = %.3f"%(c_id, stab_res.t, norm(stab_res.dc));
# raw_input();
except Exception as e:
print "ERROR while computing stability criterion:", e;
best_candidate_ids = np.where(abs(v_pred-np.min(v_pred))<EPS)[0];
if(best_candidate_ids.shape[0]>1):
# if multiple contacts result in the same final com velocity (typically that would be 0),
# pick the one who does it faster
best_candidate_id = best_candidate_ids[np.argmin(t_pred[best_candidate_ids])];
else:
best_candidate_id = best_candidate_ids[0];
return Bunch(index=best_candidate_id, c=c_pred[:,best_candidate_id],
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):
''' 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));
N[2,:] = 1.0;
stabilityCriterion = StabilityCriterion("Stab_crit", x_com, dx_com, p.T, N.T, mu, gravity, mass);
t_pred = mat_zeros(len(contacts));
t_min = mat_zeros(len(contacts));
v_pred = mat_zeros(len(contacts));
c_pred = mat_zeros((3,len(contacts)));
dc_pred = mat_zeros((3,len(contacts)));
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;
# predict future com state supposing to break name_of_contact_to_break
stabilityCriterion.set_contacts(p.T, N.T, mu);
try:
res = stabilityCriterion.predict_future_state(step_time);
t_pred[contactId] = res.t;
t_min[contactId] = res.t_min;
c_pred[:,contactId] = np.asmatrix(res.c).T;
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);
except Exception as e:
print "ERROR while computing stability criterion:", e;
t_pred_sorted = sorted(t_pred.A.squeeze());
if(t_pred_sorted[-1] > t_pred_sorted[-2]+EPS):
id_contact_to_break = np.argmax(t_pred);
else:
id_contact_to_break = np.argmin(v_pred);
name_of_contact_to_break = contacts.keys()[id_contact_to_break];
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]);
''' Solve a QP to compute initial joint velocities that satisfy contact constraints and optionally other
specified constraints, such as having the capture point inside the convex hull of the contact points.
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment