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.
......
Supports Markdown
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