class AtlasInterface(Interface): def __init__(self): super(AtlasInterface, self).__init__() if PnCConfig.DYN_LIB == "dart": from pnc.robot_system.dart_robot_system import DartRobotSystem self._robot = DartRobotSystem( cwd + "/robot_model/atlas/atlas_rel_path.urdf", False, PnCConfig.PRINT_ROBOT_INFO) elif PnCConfig.DYN_LIB == "pinocchio": from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem self._robot = PinocchioRobotSystem( cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas", False, PnCConfig.PRINT_ROBOT_INFO) else: raise ValueError("wrong dynamics library") self._sp = AtlasStateProvider(self._robot) self._se = AtlasStateEstimator(self._robot) self._control_architecture = AtlasControlArchitecture(self._robot) self._interrupt_logic = AtlasInterruptLogic(self._control_architecture) if PnCConfig.SAVE_DATA: self._data_saver = DataSaver() def get_command(self, sensor_data): if PnCConfig.SAVE_DATA: self._data_saver.add('time', self._running_time) self._data_saver.add('phase', self._control_architecture.state) # Update State Estimator if self._count == 0: print("=" * 80) print("Initialize") print("=" * 80) self._se.initialize(sensor_data) self._se.update(sensor_data) # Process Interrupt Logic self._interrupt_logic.process_interrupts() # Compute Cmd command = self._control_architecture.get_command() if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0): self._data_saver.advance() # Increase time variables self._count += 1 self._running_time += PnCConfig.CONTROLLER_DT self._sp.curr_time = self._running_time self._sp.prev_state = self._control_architecture.prev_state self._sp.state = self._control_architecture.state return copy.deepcopy(command) @property def interrupt_logic(self): return self._interrupt_logic
class LaikagoInterface(Interface): def __init__(self): super(LaikagoInterface, self).__init__() self._robot = PinocchioRobotSystem( cwd + "/robot_model/laikago/laikago_toes.urdf", cwd + "/robot_model/laikago", False, True) self._sp = LaikagoStateProvider(self._robot) self._se = LaikagoStateEstimator(self._robot) self._control_architecture = LaikagoControlArchitecture(self._robot) self._interrupt_logic = LaikagoInterruptLogic( self._control_architecture) if PnCConfig.SAVE_DATA: self._data_saver = DataSaver() def get_command(self, sensor_data): if PnCConfig.SAVE_DATA: self._data_saver.add('time', self._running_time) self._data_saver.add('phase', self._control_architecture.state) # Update State Estimator if self._count == 0: print("=" * 80) print("Initialize") print("=" * 80) self._se.initialize(sensor_data) self._se.update(sensor_data) # print("total mass") # print(self._robot.total_mass) # print("com pos") # print(self._robot.get_com_pos()) # Process Interrupt Logic self._interrupt_logic.process_interrupts() # Compute Cmd command = self._control_architecture.get_command() if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0): self._data_saver.advance() # Increase time variables self._count += 1 self._running_time += PnCConfig.CONTROLLER_DT self._sp.curr_time = self._running_time self._sp.prev_state = self._control_architecture.prev_state self._sp.state = self._control_architecture.state return copy.deepcopy(command) @property def interrupt_logic(self): return self._interrupt_logic
class PointContact(Contact): def __init__(self, robot, link_id, mu, data_save=False): super(PointContact, self).__init__(robot, 3) self._link_id = link_id self._mu = mu self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver() def _update_jacobian(self): self._jacobian = self._robot.get_link_jacobian( self._link_id)[self._dim_contact:, :] self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot( self._link_id) def _update_cone_constraint(self): rot = self._robot.get_link_iso(self._link_id)[0:3, 0:3].transpose() self._cone_constraint_mat = np.zeros((6, self._dim_contact)) self._cone_constraint_mat[0, 2] = 1. self._cone_constraint_mat[1, 0] = 1. self._cone_constraint_mat[1, 2] = self._mu self._cone_constraint_mat[2, 0] = -1. self._cone_constraint_mat[2, 2] = self._mu self._cone_constraint_mat[3, 1] = 1. self._cone_constraint_mat[3, 2] = self._mu self._cone_constraint_mat[4, 1] = -1. self._cone_constraint_mat[4, 2] = self._mu self._cone_constraint_mat[5, 2] = -1. self._cone_constraint_mat = np.dot(self._cone_constraint_mat, rot) self._cone_constraint_vec = np.zeros(6) self._cone_constraint_vec[5] = -self._rf_z_max if self._b_data_save: self._data_saver.add("rf_z_max_" + self._link_id, self._rf_z_max)
class IHWBC(object): """ Implicit Hierarchy Whole Body Control ------------------ Usage: update_setting --> solve """ def __init__(self, sf, sa, sv, data_save=False): self._n_q_dot = sa.shape[1] self._n_active = sa.shape[0] self._n_passive = sv.shape[0] self._sf = sf self._snf = np.concatenate((np.zeros( (self._n_active + self._n_passive, 6)), np.eye(self._n_active + self._n_passive)), axis=1) self._sa = sa self._sv = sv self._trq_limit = None self._lambda_q_ddot = 0. self._lambda_rf = 0. self._w_rf = 0. self._w_hierarchy = 0. self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver() @property def trq_limit(self): return self._trq_limit @property def lambda_q_ddot(self): return self._lambda_q_ddot @property def lambda_rf(self): return self._lambda_rf @property def w_hierarchy(self): return self._w_hierarchy @property def w_rf(self): return self._w_rf @trq_limit.setter def trq_limit(self, val): assert val.shape[0] == self._n_active self._trq_limit = np.copy(val) @lambda_q_ddot.setter def lambda_q_ddot(self, val): self._lambda_q_ddot = val @lambda_rf.setter def lambda_rf(self, val): self._lambda_rf = val @w_hierarchy.setter def w_hierarchy(self, val): self._w_hierarchy = val @w_hierarchy.setter def w_rf(self, val): self._w_rf = val def update_setting(self, mass_matrix, mass_matrix_inv, coriolis, gravity): self._mass_matrix = np.copy(mass_matrix) self._mass_matrix_inv = np.copy(mass_matrix_inv) self._coriolis = np.copy(coriolis) self._gravity = np.copy(gravity) def solve(self, task_list, contact_list, internal_constraint_list, rf_des=None, verbose=False): """ Parameters ---------- task_list (list of Task): Task list contact_list (list of Contact): Contact list internal_constraint_list (list of InternalConstraint): Internal constraint list rf_des (np.ndarray): Reaction force desired verbose (bool): Printing option Returns ------- joint_trq_cmd (np.array): Joint trq cmd joint_acc_cmd (np.array): Joint acc cmd sol_rf (np.array): Reaction force """ # ====================================================================== # Internal Constraint # Set ni, jit_lmd_jidot_qdot, sa_ni_trc_bar_tr, and b_internal_constraint # ====================================================================== if len(internal_constraint_list) > 0: ji = np.concatenate( [ic.jacobian for ic in internal_constraint_list], axis=0) jidot_qdot = np.concatenate( [ic.jacobian_dot_q_dot for ic in internal_constraint_list], axis=0) lmd = np.linalg.pinv( np.dot(np.dot(ji, self._mass_matrix_inv), ji.transpose())) ji_bar = np.dot(np.dot(self._mass_matrix_inv, ji.transpose()), lmd) ni = np.eye(self._n_q_dot) - np.dot(ji_bar, ji) jit_lmd_jidot_qdot = np.squeeze( np.dot(np.dot(ji.transpose(), lmd), jidot_qdot)) sa_ni_trc = np.dot(self._sa, ni)[:, 6:] sa_ni_trc_bar = util.weighted_pinv(sa_ni_trc, self._mass_matrix_inv[6:, 6:]) sa_ni_trc_bar_tr = sa_ni_trc_bar.transpose() b_internal_constraint = True else: ni = np.eye(self._n_q_dot) jit_lmd_jidot_qdot = np.zeros(self._n_q_dot) sa_ni_trc_bar = np.eye(self._n_active) sa_ni_trc_bar_tr = sa_ni_trc_bar.transpose() b_internal_constraint = False # print("ni") # print(ni) # print("jit_lmd_jidot_qdot") # print(jit_lmd_jidot_qdot) # print("sa_ni_trc_bar_tr") # print(sa_ni_trc_bar_tr) # exit() # ====================================================================== # Cost # ====================================================================== cost_t_mat = np.zeros((self._n_q_dot, self._n_q_dot)) cost_t_vec = np.zeros(self._n_q_dot) for i, task in enumerate(task_list): j = task.jacobian j_dot_q_dot = task.jacobian_dot_q_dot x_ddot = task.op_cmd if verbose: print("====================") print(task.target_id, " task") task.debug() cost_t_mat += self._w_hierarchy[i] * np.dot(j.transpose(), j) cost_t_vec += self._w_hierarchy[i] * np.dot( (j_dot_q_dot - x_ddot).transpose(), j) # cost_t_mat += self._lambda_q_ddot * np.eye(self._n_q_dot) cost_t_mat += self._lambda_q_ddot * self._mass_matrix if contact_list is not None: uf_mat = np.array( block_diag( *[contact.cone_constraint_mat for contact in contact_list])) uf_vec = np.concatenate( [contact.cone_constraint_vec for contact in contact_list]) contact_jacobian = np.concatenate( [contact.jacobian for contact in contact_list], axis=0) assert uf_mat.shape[0] == uf_vec.shape[0] assert uf_mat.shape[1] == contact_jacobian.shape[0] dim_cone_constraint, dim_contacts = uf_mat.shape cost_rf_mat = (self._lambda_rf + self._w_rf) * np.eye(dim_contacts) if rf_des is None: rf_des = np.zeros(dim_contacts) cost_rf_vec = -self._w_rf * np.copy(rf_des) cost_mat = np.array(block_diag( cost_t_mat, cost_rf_mat)) # (nqdot+nc, nqdot+nc) cost_vec = np.concatenate([cost_t_vec, cost_rf_vec]) # (nqdot+nc,) else: dim_contacts = dim_cone_constraint = 0 cost_mat = np.copy(cost_t_mat) cost_vec = np.copy(cost_t_vec) # if verbose: # print("==================================") # np.set_printoptions(precision=4) # print("cost_t_mat") # print(cost_t_mat) # print("cost_t_vec") # print(cost_t_vec) # print("cost_rf_mat") # print(cost_rf_mat) # print("cost_rf_vec") # print(cost_rf_vec) # print("cost_mat") # print(cost_mat) # print("cost_vec") # print(cost_vec) # ====================================================================== # Equality Constraint # ====================================================================== if contact_list is not None: eq_floating_mat = np.concatenate( (np.dot(self._sf, self._mass_matrix), -np.dot(self._sf, np.dot(contact_jacobian, ni).transpose())), axis=1) # (6, nqdot+nc) if b_internal_constraint: eq_int_mat = np.concatenate( (ji, np.zeros( (ji.shape[0], dim_contacts))), axis=1) # (2, nqdot+nc) eq_int_vec = np.zeros(ji.shape[0]) else: eq_floating_mat = np.dot(self._sf, self._mass_matrix) if b_internal_constraint: eq_int_mat = np.copy(ji) eq_int_vec = np.zeros(ji.shape[0]) eq_floating_vec = -np.dot( self._sf, np.dot(ni.transpose(), (self._coriolis + self._gravity))) if b_internal_constraint: eq_mat = np.concatenate((eq_floating_mat, eq_int_mat), axis=0) eq_vec = np.concatenate((eq_floating_vec, eq_int_vec), axis=0) else: eq_mat = np.copy(eq_floating_mat) eq_vec = np.copy(eq_floating_vec) # ====================================================================== # Inequality Constraint # ====================================================================== if self._trq_limit is None: if contact_list is not None: ineq_mat = np.concatenate((np.zeros( (dim_cone_constraint, self._n_q_dot)), -uf_mat), axis=1) ineq_vec = -uf_vec else: ineq_mat = None ineq_vec = None else: if contact_list is not None: ineq_mat = np.concatenate( (np.concatenate( (np.zeros((dim_cone_constraint, self._n_q_dot)), -np.dot(sa_ni_trc_bar_tr, np.dot(self._snf, self._mass_matrix)), np.dot(sa_ni_trc_bar_tr, np.dot(self._snf, self._mass_matrix))), axis=0), np.concatenate( (-uf_mat, np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(contact_jacobian, ni).transpose()), -np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(contact_jacobian, ni).transpose())), axis=0)), axis=1) ineq_vec = np.concatenate( (-uf_vec, np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(ni.transpose(), (self._coriolis + self._gravity))) + np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), jit_lmd_jidot_qdot) - self._trq_limit[:, 0], -np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(ni.transpose(), (self._coriolis + self._gravity))) - np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), jit_lmd_jidot_qdot) + self._trq_limit[:, 1])) else: ineq_mat = np.concatenate( (-np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), self._mass_matrix), np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), self._mass_matrix)), axis=0) ineq_vec = np.concatenate( (np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(ni.transpose(), (self._coriolis + self._gravity))) + np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), jit_lmd_jidot_qdot) - self._trq_limit[:, 0], -np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(ni.transpose(), (self._coriolis + self._gravity))) - np.dot(np.dot(sa_ni_trc_bar_tr, self._snf), jit_lmd_jidot_qdot) + self._trq_limit[:, 1])) # if verbose: # print("eq_mat") # print(eq_mat) # print("eq_vec") # print(eq_vec) # print("ineq_mat") # print(ineq_mat) # print("ineq_vec") # print(ineq_vec) sol = solve_qp(cost_mat, cost_vec, ineq_mat, ineq_vec, eq_mat, eq_vec, solver="quadprog", verbose=True) if contact_list is not None: sol_q_ddot, sol_rf = sol[:self._n_q_dot], sol[self._n_q_dot:] else: sol_q_ddot, sol_rf = sol, None if contact_list is not None: joint_trq_cmd = np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(self._mass_matrix, sol_q_ddot) + np.dot(ni.transpose(), (self._coriolis + self._gravity)) - np.dot(np.dot(contact_jacobian, ni).transpose(), sol_rf)) else: joint_trq_cmd = np.dot( np.dot(sa_ni_trc_bar_tr, self._snf), np.dot(self._mass_matrix, sol_q_ddot) + np.dot(ni, (self._coriolis + self._gravity))) joint_acc_cmd = np.dot(self._sa, sol_q_ddot) if verbose: # if True: print("joint_trq_cmd: ", joint_trq_cmd) print("sol_q_ddot: ", sol_q_ddot) print("sol_rf: ", sol_rf) # for i, task in enumerate(task_list): for task in [task_list[3], task_list[4]]: j = task.jacobian j_dot_q_dot = task.jacobian_dot_q_dot x_ddot = task.op_cmd print(task.target_id, " task") print("des x ddot: ", x_ddot) print("j*qddot_sol + Jdot*qdot: ", np.dot(j, sol_q_ddot) + j_dot_q_dot) if self._b_data_save: self._data_saver.add('joint_trq_cmd', joint_trq_cmd) self._data_saver.add('joint_acc_cmd', joint_acc_cmd) self._data_saver.add('rf_cmd', sol_rf) return joint_trq_cmd, joint_acc_cmd, sol_rf
class SurfaceContact(Contact): def __init__(self, robot, link_id, x, y, mu, data_save=False): super(SurfaceContact, self).__init__(robot, 6) self._link_id = link_id self._x = x self._y = y self._mu = mu self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver() def _update_jacobian(self): self._jacobian = self._robot.get_link_jacobian(self._link_id) self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot( self._link_id) def _update_cone_constraint(self): self._cone_constraint_mat = np.zeros((16 + 2, self._dim_contact)) u = self._get_u(self._x, self._y, self._mu) rot = self._robot.get_link_iso(self._link_id)[0:3, 0:3] rot_foot = np.zeros((6, 6)) rot_foot[0:3, 0:3] = rot.transpose() rot_foot[3:6, 3:6] = rot.transpose() self._cone_constraint_mat = np.dot(u, rot_foot) self._cone_constraint_vec = np.zeros(16 + 2) self._cone_constraint_vec[17] = -self._rf_z_max if self._b_data_save: self._data_saver.add("rf_z_max_" + self._link_id, self._rf_z_max) def _get_u(self, x, y, mu): u = np.zeros((16 + 2, 6)) u[0, 5] = 1. u[1, 3] = 1. u[1, 5] = mu u[2, 3] = -1. u[2, 5] = mu u[3, 4] = 1. u[3, 5] = mu u[4, 4] = -1. u[4, 5] = mu u[5, 0] = 1. u[5, 5] = y u[6, 0] = -1. u[6, 5] = y u[7, 1] = 1. u[7, 5] = x u[8, 1] = -1. u[8, 5] = x ##tau u[9, 0] = -mu u[9, 1] = -mu u[9, 2] = 1. u[9, 3] = y u[9, 4] = x u[9, 5] = (x + y) * mu u[10, 0] = -mu u[10, 1] = mu u[10, 2] = 1. u[10, 3] = y u[10, 4] = -x u[10, 5] = (x + y) * mu u[11, 0] = mu u[11, 1] = -mu u[11, 2] = 1. u[11, 3] = -y u[11, 4] = x u[11, 5] = (x + y) * mu u[12, 0] = mu u[12, 1] = mu u[12, 2] = 1. u[12, 3] = -y u[12, 4] = -x u[12, 5] = (x + y) * mu u[13, 0] = -mu u[13, 1] = -mu u[13, 2] = -1. u[13, 3] = -y u[13, 4] = -x u[13, 5] = (x + y) * mu u[14, 0] = -mu u[14, 1] = mu u[14, 2] = -1. u[14, 3] = -y u[14, 4] = x u[14, 5] = (x + y) * mu u[15, 0] = mu u[15, 1] = -mu u[15, 2] = -1. u[15, 3] = y u[15, 4] = -x u[15, 5] = (x + y) * mu u[16, 0] = mu u[16, 1] = mu u[16, 2] = -1. u[16, 3] = y u[16, 4] = x u[16, 5] = (x + y) * mu u[17, 5] = -1. return u
sensor_data["base_joint_ang_vel"], sensor_data["joint_pos"], sensor_data["joint_vel"], True) rot_world_base = util.quat_to_rot(sensor_data['base_com_quat']) world_I = robot_sys.Ig[0:3, 0:3] local_I = np.dot(np.dot(rot_world_base.transpose(), world_I), rot_world_base) rf_iso = pybullet_util.get_link_iso(robot, link_id["r_sole"]) lf_iso = pybullet_util.get_link_iso(robot, link_id["l_sole"]) denormalized_output, output = evaluate_crbi_model_using_tf( crbi_model, sensor_data["base_com_pos"], lf_iso[0:3, 3], rf_iso[0:3, 3], input_mean, input_std, output_mean, output_std) local_I_est = inertia_from_one_hot_vec(denormalized_output) if b_ik: data_saver.add('gt_inertia', inertia_to_one_hot_vec(local_I)) data_saver.add( 'gt_inertia_normalized', util.normalize(inertia_to_one_hot_vec(local_I), output_mean, output_std)) data_saver.add('est_inertia_normalized', np.copy(np.squeeze(output))) data_saver.add('est_inertia', np.copy(denormalized_output)) data_saver.advance() # Disable forward step # p.stepSimulation() time.sleep(dt) t += dt count += 1
class BasicTask(Task): def __init__(self, robot, task_type, dim, target_id=None, data_save=False): super(BasicTask, self).__init__(robot, dim) self._target_id = target_id self._task_type = task_type self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver() @property def target_id(self): return self._target_id def update_cmd(self): if self._task_type == "JOINT": pos = self._robot.joint_positions pos_err = self._pos_des - pos vel_act = self._robot.joint_velocities if self._b_data_save: self._data_saver.add('joint_pos_des', self._pos_des.copy()) self._data_saver.add('joint_vel_des', self._vel_des.copy()) self._data_saver.add('joint_pos', pos.copy()) self._data_saver.add('joint_vel', vel_act.copy()) self._data_saver.add('w_joint', self._w_hierarchy) elif self._task_type == "SELECTED_JOINT": pos = self._robot.joint_positions[self._robot.get_joint_idx( self._target_id)] pos_err = self._pos_des - pos vel_act = self._robot.joint_velocities[self._robot.get_joint_idx( self._target_id)] if self._b_data_save: self._data_saver.add('joint_pos_des', self._pos_des.copy()) self._data_saver.add('joint_vel_des', self._vel_des.copy()) self._data_saver.add('joint_pos', pos.copy()) self._data_saver.add('joint_vel', vel_act.copy()) self._data_saver.add('w_joint', self._w_hierarchy) elif self._task_type == "LINK_XYZ": pos = self._robot.get_link_iso(self._target_id)[0:3, 3] pos_err = self._pos_des - pos vel_act = self._robot.get_link_vel(self._target_id)[3:6] if self._b_data_save: self._data_saver.add(self._target_id + '_pos_des', self._pos_des.copy()) self._data_saver.add(self._target_id + '_vel_des', self._vel_des.copy()) self._data_saver.add(self._target_id + '_pos', pos.copy()) self._data_saver.add(self._target_id + '_vel', vel_act.copy()) self._data_saver.add('w_' + self._target_id, self._w_hierarchy) elif self._task_type == "LINK_ORI": quat_des = R.from_quat(self._pos_des) quat_act = R.from_matrix( self._robot.get_link_iso(self._target_id)[0:3, 0:3]) quat_err = R.from_matrix( np.dot(quat_des.as_matrix(), quat_act.as_matrix().transpose())).as_quat() pos_err = util.quat_to_exp(quat_err) vel_act = self._robot.get_link_vel(self._target_id)[0:3] if self._b_data_save: self._data_saver.add(self._target_id + '_quat_des', quat_des.as_quat()) self._data_saver.add(self._target_id + '_ang_vel_des', self._vel_des.copy()) self._data_saver.add(self._target_id + '_quat', quat_act.as_quat()) self._data_saver.add(self._target_id + '_ang_vel', vel_act.copy()) self._data_saver.add('w_' + self._target_id + "_ori", self._w_hierarchy) elif self._task_type == "COM": pos = self._robot.get_com_pos() pos_err = self._pos_des - pos vel_act = self._robot.get_com_lin_vel() if self._b_data_save: self._data_saver.add(self._target_id + '_pos_des', self._pos_des.copy()) self._data_saver.add(self._target_id + '_vel_des', self._vel_des.copy()) self._data_saver.add(self._target_id + '_pos', pos.copy()) self._data_saver.add(self._target_id + '_vel', vel_act.copy()) self._data_saver.add('w_' + self._target_id, self._w_hierarchy) else: raise ValueError for i in range(self._dim): self._op_cmd[i] = self._acc_des[i] + self._kp[i] * pos_err[ i] + self._kd[i] * (self._vel_des[i] - vel_act[i]) def update_jacobian(self): if self._task_type == "JOINT": self._jacobian[:, self._robot.n_floating:self._robot.n_floating + self._robot.n_a] = np.eye(self._dim) self._jacobian_dot_q_dot = np.zeros(self._dim) elif self._task_type == "SELECTED_JOINT": for i, jid in enumerate(self._robot.get_q_dot_idx( self._target_id)): self._jacobian[i, jid] = 1 self._jacobian_dot_q_dot = np.zeros(self._dim) elif self._task_type == "LINK_XYZ": self._jacobian = self._robot.get_link_jacobian( self._target_id)[3:6, :] self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot( self._target_id)[3:6] elif self._task_type == "LINK_ORI": self._jacobian = self._robot.get_link_jacobian( self._target_id)[0:3, :] self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot( self._target_id)[0:3] elif self._task_type == "COM": self._jacobian = self._robot.get_com_lin_jacobian() self._jacobian_dot_q_dot = np.dot( self._robot.get_com_lin_jacobian_dot(), self._robot.get_q_dot()) else: raise ValueError
class ManipulatorInterface(Interface): def __init__(self): super(ManipulatorInterface, self).__init__() if ManipulatorConfig.DYN_LIB == "dart": from pnc.robot_system.dart_robot_system import DartRobotSystem self._robot = DartRobotSystem( cwd + "/robot_model/manipulator/three_link_manipulator.urdf", True, ManipulatorConfig.PRINT_ROBOT_INFO) elif ManipulatorConfig.DYN_LIB == "pinocchio": from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem self._robot = PinocchioRobotSystem( cwd + "/robot_model/manipulator/three_link_manipulator.urdf", cwd + "/robot_model/manipulator", True, ManipulatorConfig.PRINT_ROBOT_INFO) else: raise ValueError("wrong dynamics library") self._joint_integrator = JointIntegrator(self._robot.n_a, ManipulatorConfig.DT) self._joint_integrator.pos_cutoff_freq = 0.001 # hz self._joint_integrator.vel_cutoff_freq = 0.002 # hz self._joint_integrator.max_pos_err = 0.2 # rad self._joint_integrator.joint_pos_limit = self._robot.joint_pos_limit self._joint_integrator.joint_vel_limit = self._robot.joint_vel_limit self._b_first_visit = True self._data_saver = DataSaver() def get_command(self, sensor_data): # Update Robot self._robot.update_system( sensor_data["base_com_pos"], sensor_data["base_com_quat"], sensor_data["base_com_lin_vel"], sensor_data["base_com_ang_vel"], sensor_data["base_joint_pos"], sensor_data["base_joint_quat"], sensor_data["base_joint_lin_vel"], sensor_data["base_joint_ang_vel"], sensor_data["joint_pos"], sensor_data["joint_vel"]) if self._b_first_visit: self._joint_integrator.initialize_states( self._robot.joint_velocities, self._robot.joint_positions) self._ini_ee_pos = self._robot.get_link_iso('ee')[0:3, 3] self._b_first_visit = False # Operational Space Control jpos_cmd, jvel_cmd, jtrq_cmd = self._compute_osc_command() # Compute Cmd command = self._robot.create_cmd_ordered_dict(jpos_cmd, jvel_cmd, jtrq_cmd) # Increase time variables self._count += 1 self._running_time += ManipulatorConfig.DT self._data_saver.add('time', self._running_time) self._data_saver.advance() return command def _compute_osc_command(self): jtrq = np.zeros(self._robot.n_a) jac = self._robot.get_link_jacobian('ee')[3:6, :] pos = self._robot.get_link_iso('ee')[0:3, 3] vel = self._robot.get_link_vel('ee')[3:6] pos_des = np.zeros(3) vel_des = np.zeros(3) acc_des = np.zeros(3) for i in range(3): pos_des[i] = interpolation.smooth_changing( self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3., self._running_time) vel_des[i] = interpolation.smooth_changing_vel( self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3., self._running_time) acc_des[i] = interpolation.smooth_changing_acc( self._ini_ee_pos[i], ManipulatorConfig.DES_EE_POS[i], 3., self._running_time) err = pos_des - pos err_d = vel_des - vel # xddot_des = acc_des + ManipulatorConfig.KP * err + ManipulatorConfig.KD * err_d xddot_des = ManipulatorConfig.KP * err + ManipulatorConfig.KD * err_d qddot_des = np.dot(np.linalg.pinv(jac, rcond=1e-3), xddot_des) # smoothing qddot s = interpolation.smooth_changing(0, 1, 0.5, self._running_time) qddot_des *= s joint_vel_cmd, joint_pos_cmd = self._joint_integrator.integrate( qddot_des, self._robot.joint_velocities, self._robot.joint_positions) mass_matrix = self._robot.get_mass_matrix() c = self._robot.get_coriolis() g = self._robot.get_gravity() jtrq = np.dot(mass_matrix, qddot_des) + c + g self._data_saver.add('ee_pos_des', pos_des) self._data_saver.add('ee_pos_act', pos) self._data_saver.add('ee_vel_des', vel_des) self._data_saver.add('ee_vel_act', vel) self._data_saver.add('ee_acc_des', acc_des) self._data_saver.add('jpos_des', joint_pos_cmd) self._data_saver.add('jpos_act', self._robot.joint_positions) self._data_saver.add('jvel_des', joint_vel_cmd) self._data_saver.add('jvel_act', self._robot.joint_velocities) self._data_saver.add('qddot_des', qddot_des) return joint_pos_cmd, joint_vel_cmd, jtrq
class Draco3IHWBC(object): """ Implicit Hierarchy Whole Body Control ------------------ Usage: update_setting --> solve """ def __init__(self, robot, act_list, data_save=False): self._robot = robot self._n_q_dot = len(act_list) self._n_active = np.count_nonzero(np.array(act_list)) self._n_passive = self._n_q_dot - self._n_active # Selection matrix self._sa = np.zeros((self._n_active, self._n_q_dot)) self._sv = np.zeros((self._n_passive, self._n_q_dot)) j, k = 0, 0 for i in range(self._n_q_dot): if act_list[i]: self._sa[j, i] = 1. j += 1 else: self._sv[k, i] = 1. k += 1 # Assume first six is floating self._sf = np.zeros((6, self._n_q_dot)) self._sf[0:6, 0:6] = np.eye(6) # Internal constraint self._n_int = 2 self._jac_int = np.zeros((self._n_int, self._n_q_dot)) l_jp_idx, l_jd_idx, r_jp_idx, r_jd_idx = self._robot.get_q_dot_idx( ['l_knee_fe_jp', 'l_knee_fe_jd', 'r_knee_fe_jp', 'r_knee_fe_jd']) self._jac_int[0, l_jp_idx] = 1. self._jac_int[0, l_jd_idx] = -1. self._jac_int[1, r_jp_idx] = 1. self._jac_int[1, r_jd_idx] = -1. self._trq_limit = None self._lambda_q_ddot = 0. self._lambda_if = 0. self._lambda_rf = 0. self._w_hierarchy = 0. self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver() @property def trq_limit(self): return self._trq_limit @property def lambda_q_ddot(self): return self._lambda_q_ddot @property def lambda_rf(self): return self._lambda_rf @property def lambda_if(self): return self._lambda_if @property def w_hierarchy(self): return self._w_hierarchy @trq_limit.setter def trq_limit(self, val): assert val.shape[0] == self._n_active self._trq_limit = np.copy(val) @lambda_q_ddot.setter def lambda_q_ddot(self, val): self._lambda_q_ddot = val @lambda_rf.setter def lambda_rf(self, val): self._lambda_rf = val @lambda_if.setter def lambda_if(self, val): self._lambda_if = val @w_hierarchy.setter def w_hierarchy(self, val): self._w_hierarchy = val def update_setting(self, mass_matrix, mass_matrix_inv, coriolis, gravity): self._mass_matrix = np.copy(mass_matrix) self._mass_matrix_inv = np.copy(mass_matrix_inv) self._coriolis = np.copy(coriolis) self._gravity = np.copy(gravity) def solve(self, task_list, contact_list, verbose=False): """ Parameters ---------- task_list (list of Task): Task list contact_list (list of Contact): Contact list verbose (bool): Printing option Returns ------- joint_trq_cmd (np.array): Joint trq cmd joint_acc_cmd (np.array): Joint acc cmd sol_rf (np.array): Reaction force sol_if (np.array): Internal force """ # ====================================================================== # Cost # ====================================================================== cost_t_mat = np.zeros((self._n_q_dot, self._n_q_dot)) cost_t_vec = np.zeros(self._n_q_dot) for i, task in enumerate(task_list): j = task.jacobian j_dot_q_dot = task.jacobian_dot_q_dot x_ddot = task.op_cmd if verbose: print(i, " th task") task.debug() cost_t_mat += self._w_hierarchy[i] * np.dot(j.transpose(), j) cost_t_vec += self._w_hierarchy[i] * np.dot( (j_dot_q_dot - x_ddot).transpose(), j) # cost_t_mat += self._lambda_q_ddot * np.eye(self._n_q_dot) cost_t_mat += self._lambda_q_ddot * self._mass_matrix cost_if_mat = self._lambda_if * np.eye(self._n_int) cost_if_vec = np.zeros(self._n_int) if contact_list is not None: uf_mat = np.array( block_diag( *[contact.cone_constraint_mat for contact in contact_list])) uf_vec = np.concatenate( [contact.cone_constraint_vec for contact in contact_list]) contact_jacobian = np.concatenate( [contact.jacobian for contact in contact_list], axis=0) assert uf_mat.shape[0] == uf_vec.shape[0] assert uf_mat.shape[1] == contact_jacobian.shape[0] dim_cone_constraint, dim_contacts = uf_mat.shape cost_rf_mat = self._lambda_rf * np.eye(dim_contacts) cost_rf_vec = np.zeros(dim_contacts) cost_mat = np.array( block_diag(cost_t_mat, cost_if_mat, cost_rf_mat)) # (nqdot+nint+nc, nqdot+nint+nc) cost_vec = np.concatenate([cost_t_vec, cost_if_vec, cost_rf_vec]) # (nqdot+nint+nc,) else: dim_contacts = dim_cone_constraint = 0 cost_mat = np.array(block_diag(cost_t_mat, cost_if_mat)) cost_vec = np.concatenate([cost_t_vec, cost_if_vec]) if verbose: print("cost_t_mat") print(cost_t_mat) print("cost_t_vec") print(cost_t_vec) print("cost_rf_mat") print(cost_rf_mat) print("cost_rf_vec") print(cost_rf_vec) # ====================================================================== # Equality Constraint # ====================================================================== if contact_list is not None: eq_floating_mat = np.concatenate( (np.dot(self._sf, self._mass_matrix), np.zeros( (6, self._n_int)), -np.dot(self._sf, contact_jacobian.transpose())), axis=1) # (6, nqdot+nint+nc) eq_int_mat = np.concatenate( (self._jac_int, np.zeros( (2, self._n_int)), np.zeros((2, dim_contacts))), axis=1) # (2, nqdot+nint+nc) else: eq_floating_mat = np.concatenate( (np.dot(self._sf, self._mass_matrix), np.zeros( (6, self._n_int))), axis=1) # (6, nqdot+nint) eq_int_mat = np.concatenate( (self._jac_int, np.zeros( (2, self._n_int))), axis=1) # (2, nqdot+nint) eq_floating_vec = -np.dot(self._sf, (self._coriolis + self._gravity)) eq_int_vec = np.zeros(2) eq_trq_mat = np.concatenate( (np.dot(self._jac_int, self._mass_matrix), -np.dot(self._jac_int, self._jac_int.transpose()), -np.dot(self._jac_int, contact_jacobian.transpose())), axis=1) eq_trq_vec = -np.dot(self._jac_int, (self._coriolis + self._gravity)) eq_mat = np.concatenate((eq_floating_mat, eq_trq_mat, eq_int_mat), axis=0) eq_vec = np.concatenate((eq_floating_vec, eq_trq_vec, eq_int_vec), axis=0) # ====================================================================== # Inequality Constraint # ====================================================================== if self._trq_limit is None: if contact_list is not None: ineq_mat = np.concatenate((np.zeros( (dim_cone_constraint, self._n_q_dot + self._n_int)), -uf_mat), axis=1) ineq_vec = -uf_vec else: ineq_mat = None ineq_vec = None else: if contact_list is not None: ineq_mat = np.concatenate( (np.concatenate((np.zeros( (dim_cone_constraint, self._n_q_dot)), -np.dot(self._sa, self._mass_matrix), np.dot(self._sa, self._mass_matrix)), axis=0), np.concatenate( (np.zeros((dim_cone_constraint, self._n_int)), np.dot(self._sa, self._jac_int.transpose()), -np.dot(self._sa, self._jac_int.transpose())), axis=0), np.concatenate( (-uf_mat, np.dot(self._sa, contact_jacobian.transpose()), -np.dot(self._sa, contact_jacobian.transpose())), axis=0)), axis=1) ineq_vec = np.concatenate( (-uf_vec, np.dot(self._sa, self._coriolis + self._gravity) - self._trq_limit[:, 0], -np.dot(self._sa, self._coriolis + self._gravity) + self._trq_limit[:, 1])) else: ineq_mat = np.concatenate( (np.concatenate((-np.dot(self._sa, self._mass_matrix), np.dot(self._sa, self._mass_matrix)), axis=0), np.concatenate( (np.dot(self._sa, self._jac_int.transpose()), -np.dot(self._sa, self._jac_int.transpose())), axis=0)), axis=1) ineq_vec = np.concatenate( (np.dot(self._sa, self._coriolis + self._gravity) - self._trq_limit[:, 0], -np.dot(self._sa, self._coriolis + self._gravity) + self._trq_limit[:, 1])) if verbose: print("eq_mat") print(eq_mat) print("eq_vec") print(eq_vec) print("ineq_mat") print(ineq_mat) print("ineq_vec") print(ineq_vec) sol = solve_qp(cost_mat, cost_vec, ineq_mat, ineq_vec, eq_mat, eq_vec, solver="quadprog", verbose=True) if contact_list is not None: sol_q_ddot, sol_if, sol_rf = sol[:self._n_q_dot], sol[ self._n_q_dot:self._n_q_dot + self._n_int], sol[self._n_q_dot + self._n_int:] else: sol_q_ddot, sol_if, sol_rf = sol[:self._n_q_dot], sol[ self._n_q_dot:self._n_q_dot + self._n_int], None if contact_list is not None: joint_trq_cmd = np.dot( self._sa, np.dot(self._mass_matrix, sol_q_ddot) + self._coriolis + self._gravity - np.dot(contact_jacobian.transpose(), sol_rf) - np.dot(self._jac_int.transpose(), sol_if)) else: joint_trq_cmd = np.dot( self._sa, np.dot(self._mass_matrix, sol_q_ddot) + self._coriolis + self._gravity - np.dot(self._jac_int.transpose(), sol_if)) joint_acc_cmd = np.dot(self._sa, sol_q_ddot) if verbose: print("joint_trq_cmd: ", joint_trq_cmd) print("sol_q_ddot: ", sol_q_ddot) print("sol_rf: ", sol_rf) print("sol_if: ", sol_if) if self._b_data_save: self._data_saver.add('joint_trq_cmd', joint_trq_cmd) self._data_saver.add('joint_acc_cmd', joint_acc_cmd) self._data_saver.add('if_cmd', sol_if) self._data_saver.add('rf_cmd', sol_rf) return joint_trq_cmd, joint_acc_cmd, sol_rf, sol_if
class DracoManipulationController(object): def __init__(self, tci_container, robot): self._tci_container = tci_container self._robot = robot # Initialize WBC l_jp_idx, l_jd_idx, r_jp_idx, r_jd_idx = self._robot.get_q_dot_idx( ['l_knee_fe_jp', 'l_knee_fe_jd', 'r_knee_fe_jp', 'r_knee_fe_jd']) act_list = [False] * robot.n_floating + [True] * robot.n_a act_list[l_jp_idx] = False act_list[r_jp_idx] = False n_q_dot = len(act_list) n_active = np.count_nonzero(np.array(act_list)) n_passive = n_q_dot - n_active - 6 self._sa = np.zeros((n_active, n_q_dot)) self._sv = np.zeros((n_passive, n_q_dot)) self._sd = np.zeros((n_passive, n_q_dot)) j, k = 0, 0 for i in range(n_q_dot): if i >= 6: if act_list[i]: self._sa[j, i] = 1. j += 1 else: self._sv[k, i] = 1. k += 1 self._sd[0, l_jd_idx] = 1. self._sd[1, r_jd_idx] = 1. self._sf = np.zeros((6, n_q_dot)) self._sf[0:6, 0:6] = np.eye(6) self._ihwbc = IHWBC(self._sf, self._sa, self._sv, PnCConfig.SAVE_DATA) ###consider transmission constraint # self._ihwbc = IHWBC2(self._sf, self._sa, self._sv, self._sd, # PnCConfig.SAVE_DATA) if WBCConfig.B_TRQ_LIMIT: self._ihwbc.trq_limit = np.dot(self._sa[:, 6:], self._robot.joint_trq_limit) self._ihwbc.lambda_q_ddot = WBCConfig.LAMBDA_Q_DDOT self._ihwbc.lambda_rf = WBCConfig.LAMBDA_RF # Initialize Joint Integrator self._joint_integrator = JointIntegrator(robot.n_a, PnCConfig.CONTROLLER_DT) self._joint_integrator.pos_cutoff_freq = WBCConfig.POS_CUTOFF_FREQ self._joint_integrator.vel_cutoff_freq = WBCConfig.VEL_CUTOFF_FREQ self._joint_integrator.max_pos_err = WBCConfig.MAX_POS_ERR self._joint_integrator.joint_pos_limit = self._robot.joint_pos_limit self._joint_integrator.joint_vel_limit = self._robot.joint_vel_limit self._b_first_visit = True if PnCConfig.SAVE_DATA: self._data_saver = DataSaver() def get_command(self): if self._b_first_visit: self.first_visit() # Dynamics properties mass_matrix = self._robot.get_mass_matrix() mass_matrix_inv = np.linalg.inv(mass_matrix) coriolis = self._robot.get_coriolis() gravity = self._robot.get_gravity() self._ihwbc.update_setting(mass_matrix, mass_matrix_inv, coriolis, gravity) # Task, Contact, and Internal Constraint Setup w_hierarchy_list = [] for task in self._tci_container.task_list: task.update_jacobian() task.update_cmd() w_hierarchy_list.append(task.w_hierarchy) self._ihwbc.w_hierarchy = np.array(w_hierarchy_list) for contact in self._tci_container.contact_list: contact.update_contact() for internal_constraint in self._tci_container.internal_constraint_list: internal_constraint.update_internal_constraint() # WBC commands joint_trq_cmd, joint_acc_cmd, rf_cmd = self._ihwbc.solve( self._tci_container.task_list, self._tci_container.contact_list, self._tci_container.internal_constraint_list, None, WBCConfig.VERBOSE) ###consider transmission constraint # joint_trq_cmd, joint_acc_cmd, rf_cmd = self._ihwbc.solve( # self._tci_container.task_list, self._tci_container.contact_list, # self._tci_container.internal_constraint_list, None, True, True) joint_trq_cmd = np.dot(self._sa[:, 6:].transpose(), joint_trq_cmd) joint_acc_cmd = np.dot(self._sa[:, 6:].transpose(), joint_acc_cmd) # Double integration joint_vel_cmd, joint_pos_cmd = self._joint_integrator.integrate( joint_acc_cmd, self._robot.joint_velocities, self._robot.joint_positions) if PnCConfig.SAVE_DATA: self._data_saver.add('joint_trq_cmd', joint_trq_cmd) command = self._robot.create_cmd_ordered_dict(joint_pos_cmd, joint_vel_cmd, joint_trq_cmd) return command def first_visit(self): joint_pos_ini = self._robot.joint_positions self._joint_integrator.initialize_states(np.zeros(self._robot.n_a), joint_pos_ini) self._b_first_visit = False
class DracoManipulationInterface(Interface): def __init__(self): super(DracoManipulationInterface, self).__init__() self._robot = PinocchioRobotSystem( cwd + "/robot_model/draco3/draco3.urdf", cwd + "/robot_model/draco3", False, False) self._sp = DracoManipulationStateProvider(self._robot) self._sp.initialize(self._robot) self._se = DracoManipulationStateEstimator(self._robot) self._control_architecture = DracoManipulationControlArchitecture( self._robot) self._interrupt_logic = DracoManipulationInterruptLogic( self._control_architecture) if PnCConfig.SAVE_DATA: self._data_saver = DataSaver() self._data_saver.add('joint_pos_limit', self._robot.joint_pos_limit) self._data_saver.add('joint_vel_limit', self._robot.joint_vel_limit) self._data_saver.add('joint_trq_limit', self._robot.joint_trq_limit) def get_command(self, sensor_data): if PnCConfig.SAVE_DATA: self._data_saver.add('time', self._running_time) self._data_saver.add('phase', self._control_architecture.state) # Update State Estimator if self._count == 0: # print("=" * 80) # print("Initialize") # print("=" * 80) self._se.initialize(sensor_data) self._se.update(sensor_data) # Process Interrupt Logic self._interrupt_logic.process_interrupts() # Compute Cmd command = self._control_architecture.get_command() if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0): self._data_saver.add('joint_pos', self._robot.joint_positions) self._data_saver.add('joint_vel', self._robot.joint_velocities) self._data_saver.advance() # Increase time variables self._count += 1 self._running_time += PnCConfig.CONTROLLER_DT self._sp.curr_time = self._running_time self._sp.prev_state = self._control_architecture.prev_state self._sp.state = self._control_architecture.state return copy.deepcopy(command) @property def interrupt_logic(self): return self._interrupt_logic