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 __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)