def addMesh(self, name, filename, xyz=mat_zeros(3), rpy=mat_zeros(3)):
if (ENABLE_VIEWER):
position = xyzRpyToViewerConfig(xyz, rpy)
try:
self.robot.viewer.gui.addMesh("world/" + name, filename)
except:
pass
self.robot.viewer.gui.applyConfiguration('world/' + name, position)
self.robot.viewer.gui.refresh()
def select_contact_to_make(x_com, dx_com, robot, name_contact_to_break, contacts, contact_candidates, mu, gravity, mass, viewer=None, verb=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)));
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
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(P_cand.shape[1]):
p[:,i+j] = oMi.act(P_cand[:,j]);
N[:,i+j] = oMi.rotation * N_cand[:,j];
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));
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 addSphere(self,name, radius, xyz, rpy=mat_zeros(3), color=(0,0,0,1.0), lightingMode='ON'):
#if(ENABLE_VIEWER):
position = xyzRpyToViewerConfig(xyz, rpy);
self.viewer.gui.addSphere('world/'+name, radius, color);
self.viewer.gui.applyConfiguration('world/'+name, position)
self.viewer.gui.setLightingMode('world/'+name, lightingMode);
self.viewer.gui.refresh();
def dyn_value(self, t, q, v, update_geometry=False):
# Get the current CoM position, velocity and acceleration
p_com, v_com, a_com = self.robot.com(q, v, 0 * v)
# Get reference CoM trajectory
p_ref, v_ref, a_ref = self._ref_trajectory(t)
# Compute errors
p_error = p_com - p_ref
v_error = v_com - v_ref
drift = a_com # Coriolis acceleration
a_des = -(self.kp * p_error + self.kv * v_error) + a_ref
# Compute jacobian
J = self.robot.Jcom(q)
D = mat_zeros((3, 3))
D[:, 0] = a_des / norm(a_des)
# compute 2 directions orthogonal to D[0,:] and orthogonal to each other
D[:, 1] = mat_cross(D[:, 0],
np.matrix([0.0, 0.0, 1.0]).T)
if (norm(D[:, 1]) < EPS):
D[:, 1] = mat_cross(D[:, 0],
np.matrix([0.0, 1.0, 0.0]).T)
D[:, 1] *= self.weight / norm(D[:, 1])
D[:, 2] = mat_cross(D[:, 0], D[:, 1])
D[:, 2] *= self.weight / norm(D[:, 2])
J = D.T * J
drift = D.T * drift
a_des = D.T * a_des
return J[self._mask, :], drift[self._mask], a_des[self._mask]
def config_sot_to_urdf(q):
qUrdf = mat_zeros(39)
qUrdf[:3, 0] = q[:3, 0]
quatMat = rpyToMatrix(q[3:6, 0])
quatVec = Quaternion(quatMat)
qUrdf[3:7, 0] = quatVec.coeffs()
qUrdf[7:, 0] = q[6:, 0]
return qUrdf
def _update_references(self):
t = 0.0
N = int(self._T / self._dt)
self._x_ref = mat_zeros((3, N))
self._v_ref = mat_zeros((3, N))
self._a_ref = mat_zeros((3, N))
x_err = self._M_final.translation - self._M_init.translation
for i in range(N):
td = t / self._T
td2 = td**2
td3 = td2 * td
td4 = td3 * td
td5 = td4 * td
p = 10 * td3 - 15 * td4 + 6 * td5
dp = (30 * td2 - 60 * td3 + 30 * td4) / self._T
ddp = (60 * td - 180 * td2 + 120 * td3) / self._T**2
self._x_ref[:, i] = self._M_init.translation + x_err * p
self._v_ref[:, i] = x_err * dp
self._a_ref[:, i] = x_err * ddp
t += self._dt
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);
#~ 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, 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,conf.MAX_MIN_JOINT_ACC , 100);
else:
success = True
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);
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
return success and abs(res.t - t)< EPS , res.dc, res.c, v0
def __init__(self, name, robotWrapper, robotName='robot1'):
self.name = name
self.filter = FirstOrderLowPassFilter(
0.002, self.CAMERA_LOW_PASS_FILTER_CUT_FREQUENCY, mat_zeros(2))
if (ENABLE_VIEWER):
self.robot = robotWrapper
self.robot.initDisplay("world/" + robotName, loadModel=False)
self.robot.loadDisplayModel("world/" + robotName, robotName)
self.robot.viewer.gui.setLightingMode('world/floor', 'OFF')
self.robots = {
robotName: self.robot
}
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():
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;
return (P, N);
def config_sot_to_urdf(q):
# GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qUrdf = mat_zeros(37);
qUrdf[:3,0] = q[:3,0];
quatMat = rpyToMatrix(q[3:6,0]);
quatVec = Quaternion(quatMat);
qUrdf[3:7,0] = quatVec.coeffs();
qUrdf[7:11,0] = q[18:22,0]; # chest-head
qUrdf[11:18,0] = q[29:,0]; # larm
qUrdf[18:25,0] = q[22:29,0]; # rarm
qUrdf[25:31,0] = q[12:18,0]; # lleg
qUrdf[31:,0] = q[6:12,0]; # rleg
return qUrdf;
def dyn_value(self, t, q, v, update_geometry=False):
# Get the current CoM position, velocity and acceleration
p_com, v_com, drift = self.robot.com(q, v, 0 * v)
a_des = mat_zeros(2)
# Compute jacobian
J = self.robot.Jcom(q)
# compute 2 directions orthogonal to v_com and orthogonal to each other
D = mat_zeros((3, 2))
d = v_com / norm(v_com)
D[:, 0] = mat_cross(d,
np.matrix([0.0, 0.0, 1.0]).T)
if (norm(D[:, 0]) < EPS):
D[:, 0] = mat_cross(d,
np.matrix([0.0, 1.0, 0.0]).T)
D[:, 0] /= norm(D[:, 0])
D[:, 1] = mat_cross(d, D[:, 0])
D[:, 1] /= norm(D[:, 1])
J = D.T * J
drift = D.T * drift
return J[self._mask, :], drift[self._mask], a_des[self._mask]
def config_sot_to_urdf(q):
# GEPETTO VIEWER Free flyer 0-6, CHEST HEAD 7-10, LARM 11-17, RARM 18-24, LLEG 25-30, RLEG 31-36
# ROBOT VIEWER # Free flyer0-5, RLEG 6-11, LLEG 12-17, CHEST HEAD 18-21, RARM 22-28, LARM 29-35
qUrdf = mat_zeros(37);
qUrdf[:3,0] = q[:3,0];
quatMat = rpyToMatrix(q[3:6,0]);
quatVec = Quaternion(quatMat);
qUrdf[3:7,0] = quatVec.coeffs();
qUrdf[7:11,0] = q[18:22,0]; # chest-head
qUrdf[11:18,0] = q[29:,0]; # larm
qUrdf[18:25,0] = q[22:29,0]; # rarm
qUrdf[25:31,0] = q[12:18,0]; # lleg
qUrdf[31:,0] = q[6:12,0]; # rleg
return qUrdf;
def __init__(self, name, robot=None):
if robot is None:
pass
self.name = name
self.filter = FirstOrderLowPassFilter(
0.002, self.CAMERA_LOW_PASS_FILTER_CUT_FREQUENCY, mat_zeros(2))
self.robot = robot
print "Adding robot: " + robot.name
self.initDisplay("world/pinocchio")
nodeName = "world/" + self.robot.name
self.loadDisplayModel(nodeName, "pinocchio", self.robot)
self.viewer.gui.setLightingMode('world/floor', 'OFF')
self.robots = {
robot.name: robot
}
def select_contact_to_break(x_com, dx_com, robot, mass, contacts, mu, gravity, step_time, verb=0):
''' Select which contact to break
'''
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, verb=verb);
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
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:
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);
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;
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]);
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);
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, 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, conf.MAX_MIN_JOINT_ACC, 100);
#~ r(q_hpp)
draw_q(q0);
if success:
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[:], v);
def main_new(dt=0.001, delay=0.01):
np.set_printoptions(precision=2, suppress=True)
COM_DES_1 = (0.012, 0.1, 0.81)
COM_DES_2 = (0.012, -0.1, 0.81)
dt = conf.dt
q0 = conf.q0_urdf
v0 = conf.v0
nv = v0.shape[0]
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt,
conf.model_path, conf.urdfFileName)
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS
simulator.VIEWER_DT = conf.DT_VIEWER
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3
simulator.verb = 0
robot = simulator.r
device = create_device_torque_ctrl(dt, conf.urdfFileName, conf.q0_sot)
traj_gen = create_trajectory_generator(device, dt)
estimator = create_estimator(device, dt, delay, traj_gen)
ff_locator = create_free_flyer_locator(device, estimator,
conf.urdfFileName)
# flex_est = create_flex_estimator(robot,dt);
# floatingBase = create_floatingBase(flex_est,ff_locator);
torque_ctrl = create_torque_controller(device, estimator)
pos_ctrl = create_position_controller(device, estimator, dt, traj_gen)
ctrl = create_balance_controller(device, None, estimator, torque_ctrl,
traj_gen, conf.urdfFileName, dt,
ff_locator)
plug(device.state, ctrl.q)
ctrl.init(dt, conf.urdfFileName)
ctrl.active_joints.value = conf.active_joints
ctrl_manager = create_ctrl_manager(device, torque_ctrl, pos_ctrl, ctrl,
estimator, dt)
# plug(device.jointsVelocities, torque_ctrl.jointsVelocities);
plug(device.velocity, ctrl.v)
plug(ctrl.tau_des, device.control)
t = 0.0
v = mat_zeros(nv)
dv = mat_zeros(nv)
x_rf = robot.framePosition(
robot.model.getFrameId('RLEG_JOINT5')).translation
x_lf = robot.framePosition(
robot.model.getFrameId('LLEG_JOINT5')).translation
device.displaySignals()
for i in range(conf.MAX_TEST_DURATION):
# if(norm(dv[6:24]) > 1e-8):
# print "ERROR acceleration of blocked axes is not zero:", norm(dv[6:24]);
device.increment(dt)
if (i == 1500):
ctrl.com_ref_pos.value = COM_DES_2
if (i % 10 == 0):
q = np.matrix(device.state.value).T
q_urdf = config_sot_to_urdf(q)
simulator.viewer.updateRobotConfig(q_urdf)
ctrl.f_des_right_foot.recompute(i)
ctrl.f_des_left_foot.recompute(i)
f_rf = np.matrix(ctrl.f_des_right_foot.value).T
f_lf = np.matrix(ctrl.f_des_left_foot.value).T
simulator.updateContactForcesInViewer(['rf', 'lf'], [x_rf, x_lf],
[f_rf, f_lf])
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
com[2, 0] = 0.0
simulator.updateComPositionInViewer(com)
if (i % 100 == 0):
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
v = np.matrix(device.velocity.value).T
dv = np.matrix(ctrl.dv_des.value).T
print "t=%.3f dv=%.1f v=%.1f com=" % (t, norm(dv), norm(v)), com.T,
print "zmp_lf", f_lf[3:5].T / f_lf[2, 0],
print "zmp_rf", f_rf[3:5].T / f_rf[2, 0]
if (i == 2):
ctrl_manager = ControlManager("ctrl_man")
ctrl_manager.resetProfiler()
t += dt
return (simulator, ctrl)
def joints_sot_to_urdf(q):
qUrdf = mat_zeros(32)
qUrdf = q
return qUrdf
def main(dt=0.001, delay=0.01):
np.set_printoptions(precision=3, suppress=True)
COM_DES_1 = (0.012, 0.1, 0.81)
COM_DES_2 = (0.012, 0.0, 0.81)
dt = conf.dt
q0 = conf.q0_urdf
v0 = conf.v0
nv = v0.shape[0]
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt,
conf.model_path, conf.urdfFileName)
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS
simulator.VIEWER_DT = conf.DT_VIEWER
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3
simulator.verb = 0
robot = simulator.r
device = create_device(conf.q0_sot)
from dynamic_graph.sot.core import Selec_of_vector
joint_vel = Selec_of_vector("joint_vel")
plug(device.velocity, joint_vel.sin)
joint_vel.selec(6, 36)
from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
ff_locator = FreeFlyerLocator("ffLocator")
plug(device.state, ff_locator.base6d_encoders)
plug(joint_vel.sout, ff_locator.joint_velocities)
ff_locator.init(conf.urdfFileName)
# ff_locator = create_free_flyer_locator(device, conf.urdfFileName);
# traj_gen = create_trajectory_generator(device, dt);
traj_gen = JointTrajectoryGenerator("jtg")
plug(device.state, traj_gen.base6d_encoders)
traj_gen.init(dt)
# estimator = create_estimator(device, dt, delay, traj_gen);
# torque_ctrl = create_torque_controller(device, estimator);
# pos_ctrl = create_position_controller(device, estimator, dt, traj_gen);
ctrl = InverseDynamicsBalanceController("invDynBalCtrl")
plug(device.state, ctrl.q)
plug(traj_gen.q, ctrl.posture_ref_pos)
plug(traj_gen.dq, ctrl.posture_ref_vel)
plug(traj_gen.ddq, ctrl.posture_ref_acc)
# plug(estimator.contactWrenchRightSole, ctrl.wrench_right_foot);
# plug(estimator.contactWrenchLeftSole, ctrl.wrench_left_foot);
# plug(ctrl.tau_des, torque_ctrl.jointsTorquesDesired);
# plug(ctrl.tau_des, estimator.tauDes);
ctrl.com_ref_pos.value = to_tuple(robot.com(q0))
ctrl.com_ref_pos.value = COM_DES_1
ctrl.com_ref_vel.value = 3 * (0.0, )
ctrl.com_ref_acc.value = 3 * (0.0, )
ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS
ctrl.gear_ratios.value = conf.GEAR_RATIOS
ctrl.contact_normal.value = (0.0, 0.0, 1.0)
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS
ctrl.f_min.value = conf.fMin
ctrl.mu.value = conf.mu[0]
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3)
ctrl.kp_com.value = 3 * (conf.kp_com, )
ctrl.kd_com.value = 3 * (conf.kd_com, )
ctrl.kp_constraints.value = 6 * (conf.kp_constr, )
ctrl.kd_constraints.value = 6 * (conf.kd_constr, )
ctrl.kp_posture.value = 30 * (conf.kp_posture, )
ctrl.kd_posture.value = 30 * (conf.kd_posture, )
ctrl.kp_pos.value = 30 * (0 * conf.kp_pos, )
ctrl.kd_pos.value = 30 * (0 * conf.kd_pos, )
ctrl.w_com.value = conf.w_com
ctrl.w_forces.value = conf.w_forces
ctrl.w_posture.value = conf.w_posture
ctrl.w_base_orientation.value = conf.w_base_orientation
ctrl.w_torques.value = conf.w_torques
ctrl.init(dt, conf.urdfFileName)
ctrl.active_joints.value = conf.active_joints
# ctrl_manager = create_ctrl_manager(device, torque_ctrl, pos_ctrl, ctrl, estimator, dt);
# plug(device.velocity, torque_ctrl.jointsVelocities);
plug(device.velocity, ctrl.v)
plug(ctrl.dv_des, device.control)
t = 0.0
v = mat_zeros(nv)
dv = mat_zeros(nv)
x_rf = robot.framePosition(
robot.model.getFrameId('RLEG_JOINT5')).translation
x_lf = robot.framePosition(
robot.model.getFrameId('LLEG_JOINT5')).translation
simulator.viewer.addSphere('zmp_rf', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.3, 1.0, 1.0), 'OFF')
simulator.viewer.addSphere('zmp_lf', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.3, 1.0, 1.0), 'OFF')
simulator.viewer.addSphere('zmp', 0.01, mat_zeros(3), mat_zeros(3),
(0.8, 0.8, 0.3, 1.0), 'OFF')
for i in range(conf.MAX_TEST_DURATION):
# if(norm(dv[6:24]) > 1e-8):
# print "ERROR acceleration of blocked axes is not zero:", norm(dv[6:24]);
ff_locator.base6dFromFoot_encoders.recompute(i)
ff_locator.v.recompute(i)
device.increment(dt)
if (i == 1500):
ctrl.com_ref_pos.value = COM_DES_2
if (i % 30 == 0):
q = np.matrix(device.state.value).T
q_urdf = config_sot_to_urdf(q)
simulator.viewer.updateRobotConfig(q_urdf)
ctrl.f_des_right_foot.recompute(i)
ctrl.f_des_left_foot.recompute(i)
f_rf = np.matrix(ctrl.f_des_right_foot.value).T
f_lf = np.matrix(ctrl.f_des_left_foot.value).T
simulator.updateContactForcesInViewer(['rf', 'lf'], [x_rf, x_lf],
[f_rf, f_lf])
ctrl.zmp_des_right_foot.recompute(i)
ctrl.zmp_des_left_foot.recompute(i)
ctrl.zmp_des.recompute(i)
zmp_rf = np.matrix(ctrl.zmp_des_right_foot.value).T
zmp_lf = np.matrix(ctrl.zmp_des_left_foot.value).T
zmp = np.matrix(ctrl.zmp_des.value).T
simulator.viewer.updateObjectConfig(
'zmp_rf', (zmp_rf[0, 0], zmp_rf[1, 0], 0., 0., 0., 0., 1.))
simulator.viewer.updateObjectConfig(
'zmp_lf', (zmp_lf[0, 0], zmp_lf[1, 0], 0., 0., 0., 0., 1.))
simulator.viewer.updateObjectConfig(
'zmp', (zmp[0, 0], zmp[1, 0], 0., 0., 0., 0., 1.))
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
com[2, 0] = 0.0
simulator.updateComPositionInViewer(com)
if (i % 100 == 0):
ctrl.com.recompute(i)
com = np.matrix(ctrl.com.value).T
v = np.matrix(device.velocity.value).T
dv = np.matrix(ctrl.dv_des.value).T
ctrl.zmp_des_right_foot_local.recompute(i)
ctrl.zmp_des_left_foot_local.recompute(i)
ctrl.zmp_des.recompute(i)
zmp_rf = np.matrix(ctrl.zmp_des_right_foot_local.value).T
zmp_lf = np.matrix(ctrl.zmp_des_left_foot_local.value).T
zmp = np.matrix(ctrl.zmp_des.value).T
print "t=%.3f dv=%.1f v=%.1f com=" % (t, norm(dv), norm(v)), com.T
print "zmp_lf", np.array([-f_lf[4, 0], f_lf[3, 0]
]) / f_lf[2, 0], zmp_lf.T
print "zmp_rf", np.array([-f_rf[4, 0], f_rf[3, 0]
]) / f_rf[2, 0], zmp_rf.T, '\n'
# print "Base pos (real)", device.state.value[:3];
# print "Base pos (esti)", ff_locator.base6dFromFoot_encoders.value[:3], '\n';
# print "Base velocity (real)", np.array(device.velocity.value[:6]);
# print "Base velocity (esti)", np.array(ff_locator.v.value[:6]), '\n';
if (i == 2):
ctrl_manager = ControlManager("ctrl_man")
ctrl_manager.resetProfiler()
t += dt
sleep(dt)
return (simulator, ctrl)
def main_new(dt=0.001, delay=0.01):
np.set_printoptions(precision=2, suppress=True);
COM_DES_1 = (0.012, 0.1, 0.81);
COM_DES_2 = (0.012, -0.1, 0.81);
dt = conf.dt;
q0 = conf.q0_urdf;
v0 = conf.v0;
nv = v0.shape[0];
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt, conf.model_path, conf.urdfFileName);
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS;
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS;
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS;
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
simulator.VIEWER_DT = conf.DT_VIEWER;
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
simulator.verb=0;
robot = simulator.r;
device = create_device_torque_ctrl(dt, conf.urdfFileName, conf.q0_sot);
traj_gen = create_trajectory_generator(device, dt);
estimator = create_estimator(device, dt, delay, traj_gen);
ff_locator = create_free_flyer_locator(device, estimator, conf.urdfFileName);
# flex_est = create_flex_estimator(robot,dt);
# floatingBase = create_floatingBase(flex_est,ff_locator);
torque_ctrl = create_torque_controller(device, estimator);
pos_ctrl = create_position_controller(device, estimator, dt, traj_gen);
ctrl = create_balance_controller(device, None, estimator, torque_ctrl, traj_gen, conf.urdfFileName, dt, ff_locator);
plug(device.state, ctrl.q);
ctrl.init(dt, conf.urdfFileName);
ctrl.active_joints.value = conf.active_joints;
ctrl_manager = create_ctrl_manager(device, torque_ctrl, pos_ctrl, ctrl, estimator, dt);
# plug(device.jointsVelocities, torque_ctrl.jointsVelocities);
plug(device.velocity, ctrl.v);
plug(ctrl.tau_des, device.control);
t = 0.0;
v = mat_zeros(nv);
dv = mat_zeros(nv);
x_rf = robot.framePosition(robot.model.getFrameId('RLEG_JOINT5')).translation;
x_lf = robot.framePosition(robot.model.getFrameId('LLEG_JOINT5')).translation;
device.displaySignals()
for i in range(conf.MAX_TEST_DURATION):
# if(norm(dv[6:24]) > 1e-8):
# print "ERROR acceleration of blocked axes is not zero:", norm(dv[6:24]);
device.increment(dt);
if(i==1500):
ctrl.com_ref_pos.value = COM_DES_2;
if(i%10==0):
q = np.matrix(device.state.value).T;
q_urdf = config_sot_to_urdf(q);
simulator.viewer.updateRobotConfig(q_urdf);
ctrl.f_des_right_foot.recompute(i);
ctrl.f_des_left_foot.recompute(i);
f_rf = np.matrix(ctrl.f_des_right_foot.value).T
f_lf = np.matrix(ctrl.f_des_left_foot.value).T
simulator.updateContactForcesInViewer(['rf', 'lf'],
[x_rf, x_lf],
[f_rf, f_lf]);
ctrl.com.recompute(i);
com = np.matrix(ctrl.com.value).T
com[2,0] = 0.0;
simulator.updateComPositionInViewer(com);
if(i%100==0):
ctrl.com.recompute(i);
com = np.matrix(ctrl.com.value).T
v = np.matrix(device.velocity.value).T;
dv = np.matrix(ctrl.dv_des.value).T;
print "t=%.3f dv=%.1f v=%.1f com=" % (t, norm(dv), norm(v)), com.T,
print "zmp_lf", f_lf[3:5].T/f_lf[2,0],
print "zmp_rf", f_rf[3:5].T/f_rf[2,0];
if(i==2):
ctrl_manager = ControlManager("ctrl_man");
ctrl_manager.resetProfiler();
t += dt;
return (simulator, ctrl);
def main(dt=0.001, delay=0.01):
np.set_printoptions(precision=3, suppress=True);
COM_DES_1 = (0.012, 0.1, 0.81);
COM_DES_2 = (0.012, 0.0, 0.81);
dt = conf.dt;
q0 = conf.q0_urdf;
v0 = conf.v0;
nv = v0.shape[0];
simulator = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt, conf.model_path, conf.urdfFileName);
simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS;
simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS;
simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS;
simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
simulator.VIEWER_DT = conf.DT_VIEWER;
simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
simulator.verb=0;
robot = simulator.r;
device = create_device(conf.q0_sot);
from dynamic_graph.sot.core import Selec_of_vector
joint_vel = Selec_of_vector("joint_vel");
plug(device.velocity, joint_vel.sin);
joint_vel.selec(6, 36);
from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
ff_locator = FreeFlyerLocator("ffLocator");
plug(device.state, ff_locator.base6d_encoders);
plug(joint_vel.sout, ff_locator.joint_velocities);
ff_locator.init(conf.urdfFileName);
# ff_locator = create_free_flyer_locator(device, conf.urdfFileName);
# traj_gen = create_trajectory_generator(device, dt);
traj_gen = JointTrajectoryGenerator("jtg");
plug(device.state, traj_gen.base6d_encoders);
traj_gen.init(dt);
# estimator = create_estimator(device, dt, delay, traj_gen);
# torque_ctrl = create_torque_controller(device, estimator);
# pos_ctrl = create_position_controller(device, estimator, dt, traj_gen);
ctrl = InverseDynamicsBalanceController("invDynBalCtrl");
plug(device.state, ctrl.q);
plug(traj_gen.q, ctrl.posture_ref_pos);
plug(traj_gen.dq, ctrl.posture_ref_vel);
plug(traj_gen.ddq, ctrl.posture_ref_acc);
# plug(estimator.contactWrenchRightSole, ctrl.wrench_right_foot);
# plug(estimator.contactWrenchLeftSole, ctrl.wrench_left_foot);
# plug(ctrl.tau_des, torque_ctrl.jointsTorquesDesired);
# plug(ctrl.tau_des, estimator.tauDes);
ctrl.com_ref_pos.value = to_tuple(robot.com(q0));
ctrl.com_ref_pos.value = COM_DES_1;
ctrl.com_ref_vel.value = 3*(0.0,);
ctrl.com_ref_acc.value = 3*(0.0,);
ctrl.rotor_inertias.value = conf.ROTOR_INERTIAS;
ctrl.gear_ratios.value = conf.GEAR_RATIOS;
ctrl.contact_normal.value = (0.0, 0.0, 1.0);
ctrl.contact_points.value = conf.RIGHT_FOOT_CONTACT_POINTS;
ctrl.f_min.value = conf.fMin;
ctrl.mu.value = conf.mu[0];
ctrl.weight_contact_forces.value = (1e2, 1e2, 1e0, 1e3, 1e3, 1e3);
ctrl.kp_com.value = 3*(conf.kp_com,);
ctrl.kd_com.value = 3*(conf.kd_com,);
ctrl.kp_constraints.value = 6*(conf.kp_constr,);
ctrl.kd_constraints.value = 6*(conf.kd_constr,);
ctrl.kp_posture.value = 30*(conf.kp_posture,);
ctrl.kd_posture.value = 30*(conf.kd_posture,);
ctrl.kp_pos.value = 30*(0*conf.kp_pos,);
ctrl.kd_pos.value = 30*(0*conf.kd_pos,);
ctrl.w_com.value = conf.w_com;
ctrl.w_forces.value = conf.w_forces;
ctrl.w_posture.value = conf.w_posture;
ctrl.w_base_orientation.value = conf.w_base_orientation;
ctrl.w_torques.value = conf.w_torques;
ctrl.init(dt, conf.urdfFileName);
ctrl.active_joints.value = conf.active_joints;
# ctrl_manager = create_ctrl_manager(device, torque_ctrl, pos_ctrl, ctrl, estimator, dt);
# plug(device.velocity, torque_ctrl.jointsVelocities);
plug(device.velocity, ctrl.v);
plug(ctrl.dv_des, device.control);
t = 0.0;
v = mat_zeros(nv);
dv = mat_zeros(nv);
x_rf = robot.framePosition(robot.model.getFrameId('RLEG_JOINT5')).translation;
x_lf = robot.framePosition(robot.model.getFrameId('LLEG_JOINT5')).translation;
simulator.viewer.addSphere('zmp_rf', 0.01, mat_zeros(3), mat_zeros(3), (0.8, 0.3, 1.0, 1.0), 'OFF');
simulator.viewer.addSphere('zmp_lf', 0.01, mat_zeros(3), mat_zeros(3), (0.8, 0.3, 1.0, 1.0), 'OFF');
simulator.viewer.addSphere('zmp', 0.01, mat_zeros(3), mat_zeros(3), (0.8, 0.8, 0.3, 1.0), 'OFF');
for i in range(conf.MAX_TEST_DURATION):
# if(norm(dv[6:24]) > 1e-8):
# print "ERROR acceleration of blocked axes is not zero:", norm(dv[6:24]);
ff_locator.base6dFromFoot_encoders.recompute(i);
ff_locator.v.recompute(i);
device.increment(dt);
if(i==1500):
ctrl.com_ref_pos.value = COM_DES_2;
if(i%30==0):
q = np.matrix(device.state.value).T;
q_urdf = config_sot_to_urdf(q);
simulator.viewer.updateRobotConfig(q_urdf);
ctrl.f_des_right_foot.recompute(i);
ctrl.f_des_left_foot.recompute(i);
f_rf = np.matrix(ctrl.f_des_right_foot.value).T
f_lf = np.matrix(ctrl.f_des_left_foot.value).T
simulator.updateContactForcesInViewer(['rf', 'lf'],
[x_rf, x_lf],
[f_rf, f_lf]);
ctrl.zmp_des_right_foot.recompute(i);
ctrl.zmp_des_left_foot.recompute(i);
ctrl.zmp_des.recompute(i);
zmp_rf = np.matrix(ctrl.zmp_des_right_foot.value).T;
zmp_lf = np.matrix(ctrl.zmp_des_left_foot.value).T;
zmp = np.matrix(ctrl.zmp_des.value).T;
simulator.viewer.updateObjectConfig('zmp_rf', (zmp_rf[0,0], zmp_rf[1,0], 0., 0.,0.,0.,1.));
simulator.viewer.updateObjectConfig('zmp_lf', (zmp_lf[0,0], zmp_lf[1,0], 0., 0.,0.,0.,1.));
simulator.viewer.updateObjectConfig('zmp', (zmp[0,0], zmp[1,0], 0., 0.,0.,0.,1.));
ctrl.com.recompute(i);
com = np.matrix(ctrl.com.value).T
com[2,0] = 0.0;
simulator.updateComPositionInViewer(com);
if(i%100==0):
ctrl.com.recompute(i);
com = np.matrix(ctrl.com.value).T
v = np.matrix(device.velocity.value).T;
dv = np.matrix(ctrl.dv_des.value).T;
ctrl.zmp_des_right_foot_local.recompute(i);
ctrl.zmp_des_left_foot_local.recompute(i);
ctrl.zmp_des.recompute(i);
zmp_rf = np.matrix(ctrl.zmp_des_right_foot_local.value).T;
zmp_lf = np.matrix(ctrl.zmp_des_left_foot_local.value).T;
zmp = np.matrix(ctrl.zmp_des.value).T;
print "t=%.3f dv=%.1f v=%.1f com=" % (t, norm(dv), norm(v)), com.T;
print "zmp_lf", np.array([-f_lf[4,0], f_lf[3,0]])/f_lf[2,0], zmp_lf.T;
print "zmp_rf", np.array([-f_rf[4,0], f_rf[3,0]])/f_rf[2,0], zmp_rf.T, '\n';
# print "Base pos (real)", device.state.value[:3];
# print "Base pos (esti)", ff_locator.base6dFromFoot_encoders.value[:3], '\n';
# print "Base velocity (real)", np.array(device.velocity.value[:6]);
# print "Base velocity (esti)", np.array(ff_locator.v.value[:6]), '\n';
if(i==2):
ctrl_manager = ControlManager("ctrl_man");
ctrl_manager.resetProfiler();
t += dt;
sleep(dt);
return (simulator, ctrl);
def compute_initial_joint_velocities_multi_contact(q, robot, contacts, constraint_mask, mu,
ZERO_INITIAL_ANGULAR_MOMENTUM,
ZERO_INITIAL_VERTICAL_COM_VEL,
ENSURE_STABILITY, ENSURE_UNSTABILITY,
MAX_INITIAL_COM_VEL, MAX_MIN_JOINT_ACC, MAX_ITER):
nv = robot.nv;
na = robot.nv-6;
robot.computeAllTerms(q, mat_zeros(nv));
robot.framesKinematics(q);
x_com = robot.com(q, update_kinematics=False);
J_com = robot.Jcom(q, update_kinematics=False);
M = robot.mass(q, update_kinematics=False);
constr_dim = constraint_mask.sum();
k = int(np.sum([constr_dim for con in contacts]));
Jc = mat_zeros((k,nv));
i = 0;
for name in contacts:
fid = robot.model.getFrameId(name);
Jc[i:i+constr_dim,:] = robot.frameJacobian(q, fid, False);
i += constr_dim;
(P,N) = compute_contact_points_from_contact_dictionary(robot, contacts);
n_in = k+3 if ZERO_INITIAL_ANGULAR_MOMENTUM else k;
n_in += 1 if ZERO_INITIAL_VERTICAL_COM_VEL else 0;
A = J_com;
A_in = mat_zeros((n_in,nv));
lb_in = mat_zeros(n_in);
ub_in = mat_zeros(n_in);
A_in[:k,:] = Jc;
if(ZERO_INITIAL_ANGULAR_MOMENTUM):
A_in[k:k+3,:] = M[3:6,:];
if(ZERO_INITIAL_VERTICAL_COM_VEL):
A_in[-1,:] = J_com[2,:];
# compute joint velocity bounds for viability
v_lb = mat_zeros(nv)-1e10;
v_ub = mat_zeros(nv)+1e10;
qMin = robot.model.lowerPositionLimit;
qMax = robot.model.upperPositionLimit;
dqMax = robot.model.velocityLimit;
for j in range(na):
(v_lb[6+j], v_ub[6+j]) = computeVelLimits(q[7+j], qMin[7+j], qMax[7+j], dqMax[6+j], MAX_MIN_JOINT_ACC);
# verify that given CoM position allows for static equilibrium
dx_com_des = mat_zeros(3);
v0 = mat_zeros(3);
stab_criterion = StabilityCriterion("default", x_com, dx_com_des, P.T, N.T, mu, np.array([0,0,-9.81]), M[0,0]);
try:
# check initial state is in static equilibrium
res = stab_criterion.can_I_stop();
if(not res.is_stable):
print "[compute_initial_joint_velocities_multi_contact] ERROR: initial configuration is not in static equilibrium", res.c, res.dc;
return (False, v0);
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while testing static equilibrium. %s"%str(e);
# sample desired CoM velocities
initial_state_generated = False;
counter = 0;
while(not initial_state_generated):
counter += 1;
if(counter > MAX_ITER):
return (False, v0);
dx_com_des = MAX_INITIAL_COM_VEL * mat_rand(3) / sqrt(3.0);
if(ENSURE_STABILITY or ENSURE_UNSTABILITY):
try:
res = stab_criterion.can_I_stop(x_com, dx_com_des);
if((ENSURE_STABILITY and not res.is_stable) or (ENSURE_UNSTABILITY and res.is_stable)):
continue;
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while checking stability of desired com velocity. %s"%str(e);
continue;
# solve QP to find joint velocities
(imode,v0) = solveLeastSquare(A.A, dx_com_des.A, v_lb.A, v_ub.A, A_in.A, lb_in.A, ub_in.A, regularization=1e-4);
if(imode==0):
dx_com_0 = np.dot(J_com, v0);
try:
res = stab_criterion.can_I_stop(x_com, dx_com_0);
if((ENSURE_UNSTABILITY and not res.is_stable) or (ENSURE_STABILITY and res.is_stable) or
(not ENSURE_STABILITY and not ENSURE_UNSTABILITY)):
initial_state_generated = True;
except Exception as e:
print "[compute_initial_joint_velocities_multi_contact] Error while checking stability of com velocity. %s"%str(e);
return (True, v0);
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
return success and abs(res.t - t)< EPS , 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());
else:
robot = RobotWrapper(conf.urdfFileName, conf.model_path, None);
nq = robot.nq;
nv = robot.nv;
v0 = mat_zeros(nv);
invDynForm = None;
robot = None;
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 = RobotWrapper( conf.urdfFileName, conf.model_path, root_joint=se3.JointModelFreeFlyer());
simulator = createSimulator(q0, v0);
#~ gen_com_vel(q0, config_test['contact_points'])
def compute_initial_joint_velocities(q, invDynForm, ZERO_INITIAL_ANGULAR_MOMENTUM, ZERO_INITIAL_VERTICAL_COM_VEL,
ENSURE_INITIAL_CAPTURE_POINT_OUT, ENSURE_INITIAL_CAPTURE_POINT_IN,
MAX_INITIAL_COM_VEL, MAX_ITER):
nv = invDynForm.nv;
na = invDynForm.na;
(B_ch, b_ch) = invDynForm.getSupportPolygon();
k = invDynForm.Jc.shape[0];
n_in = k+3 if ZERO_INITIAL_ANGULAR_MOMENTUM else k;
n_in += 1 if ZERO_INITIAL_VERTICAL_COM_VEL else 0;
A = np.matrix.copy(invDynForm.J_com);
A_in = mat_zeros((n_in,nv));
lb_in = mat_zeros(n_in);
ub_in = mat_zeros(n_in);
A_in[:k,:] = invDynForm.Jc;
if(ZERO_INITIAL_ANGULAR_MOMENTUM):
A_in[k:k+3,:] = invDynForm.M[3:6,:];
if(ZERO_INITIAL_VERTICAL_COM_VEL):
A_in[-1,:] = invDynForm.J_com[2,:];
# compute joint velocity bounds for viability
v_lb = mat_zeros(nv)-1e10;
v_ub = mat_zeros(nv)+1e10;
for j in range(na):
(v_lb[6+j], v_ub[6+j]) = computeVelLimits(q[7+j], invDynForm.qMin[7+j], invDynForm.qMax[7+j], invDynForm.dqMax[6+j], invDynForm.MAX_MIN_JOINT_ACC);
# sample desired capture point position
p = np.matlib.copy(invDynForm.contact_points);
cp_ub = np.matrix([ np.max(p[:,0])+0.1, np.max(p[:,1])+0.1 ]).T;
cp_lb = np.matrix([ np.min(p[:,0])-0.1, np.min(p[:,1])-0.1 ]).T;
dx_com_des = mat_zeros(3);
initial_state_generated = False;
counter = 0;
v0 = mat_zeros(3);
while(not initial_state_generated):
counter += 1;
if(counter > MAX_ITER):
return (False, v0);
cp_des = cp_lb + np.multiply(mat_rand(2), (cp_ub-cp_lb));
cp_ineq = np.min(np.dot(B_ch, cp_des) + b_ch);
if((ENSURE_INITIAL_CAPTURE_POINT_OUT and cp_ineq>=0.0) or (ENSURE_INITIAL_CAPTURE_POINT_IN and cp_ineq<0.0)):
continue;
# compute com vel corresponding to desired capture point
dx_com_des[:2] = sqrt(9.81/invDynForm.x_com[2]) * (cp_des - invDynForm.x_com[:2]);
if(norm(dx_com_des)>=MAX_INITIAL_COM_VEL):
continue;
# solve QP to find joint velocities
(imode,v0) = solveLeastSquare(A.A, dx_com_des.A, v_lb.A, v_ub.A, A_in.A, lb_in.A, ub_in.A, regularization=1e-4);
if(imode==0):
dx_com_0 = np.dot(invDynForm.J_com, v0);
cp_0 = invDynForm.x_com[:2] + dx_com_0[:2] / sqrt(9.81/invDynForm.x_com[2,0]);
cp_ineq = np.min(np.dot(B_ch, cp_0) + b_ch);
if((ENSURE_INITIAL_CAPTURE_POINT_OUT and cp_ineq<0.0) or
(ENSURE_INITIAL_CAPTURE_POINT_IN and cp_ineq>=0.0) or
(ENSURE_INITIAL_CAPTURE_POINT_OUT==False and ENSURE_INITIAL_CAPTURE_POINT_IN==False)):
initial_state_generated = True;
return (True, v0);
robot.initDisplay(loadModel=True)
#robot.loadDisplayModel("world/pinocchio", "pinocchio", model_path)
robot.viewer.gui.setLightingMode('world/floor', 'OFF')
#print robot.model
Q_MIN = robot.model.lowerPositionLimit
Q_MAX = robot.model.upperPositionLimit
DQ_MAX = robot.model.velocityLimit
TAU_MAX = robot.model.effortLimit
q = se3.randomConfiguration(robot.model, Q_MIN, Q_MAX)
robot.display(q)
# Display the robot in Gepetto-Viewer.
nq = robot.nq
nv = robot.nv
v = mat_zeros(robot.nv)
dv = mat_zeros(robot.nv)
ddq_ub_min = mat_zeros(nq) + 1e10
ddq_lb_max = mat_zeros(nq) - 1e10
M_max = mat_zeros((nv, nv)) - 1e10
M_min = mat_zeros((nv, nv)) + 1e10
h_max = mat_zeros(nv) - 1e10
h_min = mat_zeros(nv) + 1e10
dh_dq_max = mat_zeros((nv, nq)) - 1e10
dh_dq_min = mat_zeros((nv, nq)) + 1e10
dh_dv_max = mat_zeros((nv, nv)) - 1e10
dh_dv_min = mat_zeros((nv, nv)) + 1e10
q_h_max = mat_zeros((nq, nv))
q_h_min = mat_zeros((nq, nv))
q_M_max = createListOfLists(nv, nv)
def updateObjectConfigRpy(self, name, xyz=mat_zeros(3), rpy=mat_zeros(3)):
assert np.asmatrix(xyz).dtype == float
assert np.asmatrix(rpy).dtype == float
if (ENABLE_VIEWER):
config = xyzRpyToViewerConfig(xyz, rpy)
self.updateObjectConfig(name, config)
