class ManipulatorInterface(Interface): def __init__(self): super(ManipulatorInterface, self).__init__() self._robot = PinocchioRobotSystem( cwd + "/robot_model/manipulator/three_link_manipulator.urdf", cwd + "/robot_model/manipulator", True, ManipulatorConfig.PRINT_ROBOT_INFO) 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"]) # Operational Space Control jtrq_cmd = self._compute_osc_command() jpos_cmd = np.zeros_like(jtrq_cmd) jvel_cmd = np.zeros_like(jtrq_cmd) # 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 return command def _compute_osc_command(self): ## TODO : Implement Operational Space Control jtrq = np.zeros(self._robot.n_a) return jtrq
def _do_generate_data(n_data, nominal_lf_iso, nominal_rf_iso, nominal_sensor_data, side, rseed=None, cpu_idx=0): if rseed is not None: np.random.seed(rseed) from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem robot_sys = PinocchioRobotSystem(cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas", False, False) data_x, data_y = [], [] text = "#" + "{}".format(cpu_idx).zfill(3) with tqdm(total=n_data, desc=text + ': Generating data', position=cpu_idx + 1) as pbar: for i in range(n_data): swing_time, lfoot_ini_iso, lfoot_mid_iso, lfoot_fin_iso, lfoot_mid_vel, rfoot_ini_iso, rfoot_mid_iso, rfoot_fin_iso, rfoot_mid_vel, base_ini_iso, base_fin_iso = sample_swing_config( nominal_lf_iso, nominal_rf_iso, side) lfoot_pos_curve_ini_to_mid, lfoot_pos_curve_mid_to_fin, lfoot_quat_curve, rfoot_pos_curve_ini_to_mid, rfoot_pos_curve_mid_to_fin, rfoot_quat_curve, base_pos_curve, base_quat_curve = create_curves( lfoot_ini_iso, lfoot_mid_iso, lfoot_fin_iso, lfoot_mid_vel, rfoot_ini_iso, rfoot_mid_iso, rfoot_fin_iso, rfoot_mid_vel, base_ini_iso, base_fin_iso) for s in np.linspace(0, 1, N_DATA_PER_MOTION): base_pos = base_pos_curve.evaluate(s) base_quat = base_quat_curve.evaluate(s) if s <= 0.5: sprime = 2.0 * s lf_pos = lfoot_pos_curve_ini_to_mid.evaluate(sprime) rf_pos = rfoot_pos_curve_ini_to_mid.evaluate(sprime) else: sprime = 2.0 * (s - 0.5) lf_pos = lfoot_pos_curve_mid_to_fin.evaluate(sprime) rf_pos = rfoot_pos_curve_mid_to_fin.evaluate(sprime) lf_quat = lfoot_quat_curve.evaluate(s) rf_quat = rfoot_quat_curve.evaluate(s) joint_pos, lf_done, rf_done = ik_feet( base_pos, base_quat, lf_pos, lf_quat, rf_pos, rf_quat, nominal_sensor_data, joint_screws_in_ee_at_home, ee_SE3_at_home, open_chain_joints) if lf_done and rf_done: rot_world_com = util.quat_to_rot(np.copy(base_quat)) rot_world_joint = np.dot( rot_world_com, rot_basejoint_to_basecom.transpose()) base_joint_pos = base_pos - np.dot( rot_world_joint, pos_basejoint_to_basecom) base_joint_quat = util.rot_to_quat(rot_world_joint) robot_sys.update_system(base_pos, base_quat, np.zeros(3), np.zeros(3), base_joint_pos, base_joint_quat, np.zeros(3), np.zeros(3), joint_pos, nominal_sensor_data['joint_vel'], True) world_I = robot_sys.Ig[0:3, 0:3] local_I = np.dot( np.dot(rot_world_com.transpose(), world_I), rot_world_com) # append to data data_x.append( np.concatenate([lf_pos - base_pos, rf_pos - base_pos], axis=0)) data_y.append( np.array([ local_I[0, 0], local_I[1, 1], local_I[2, 2], local_I[0, 1], local_I[0, 2], local_I[1, 2] ])) pbar.update(1) return data_x, data_y
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
# Arbitrary q and qdot q = np.array([0.2, 0.1]) qdot = np.array([0.012323, 0.01]) # Update Robot dart_robot_sys.update_system(None, None, None, None, None, None, None, None, { 'j0': q[0], 'j1': q[1] }, { 'j0': qdot[0], 'j1': qdot[1] }) pin_robot_sys.update_system(None, None, None, None, None, None, None, None, { 'j0': q[0], 'j1': q[1] }, { 'j0': qdot[0], 'j1': qdot[1] }) # Compare EE SE(3) print("=" * 80) print("EE SE3") print("=" * 80) print(dart_robot_sys.get_link_iso('ee')) print(pin_robot_sys.get_link_iso('ee')) # Compare jacobian jac_pin = pin_robot_sys.get_link_jacobian('ee') jac_dart = dart_robot_sys.get_link_jacobian('ee') an_jac = analytic_jacobian(q)
if VIDEO_RECORD: pybullet_util.make_video(video_dir, False) p.disconnect() exit() else: vis_idx += 1 # Visualize config pybullet_util.set_config(robot, joint_id, link_id, base_pos, base_quat, joint_pos) if b_crbi and VIDEO_RECORD: robot_sys.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"], 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)
s_a = np.zeros((robot.n_a - 2, robot.n_a)) act_list = [False] * robot.n_floating + [True] * robot.n_a act_list[l_jp_idx] = False act_list[r_jp_idx] = False j, k = 0, 0 for i in range(robot.n_q_dot): if act_list[i]: s_a[j, i] = 1. j += 1 # print(j_i) # print(s_a) # exit() robot.update_system(np.zeros(3), np.zeros(4), np.zeros(3), np.zeros(3), np.zeros(3), np.zeros(4), np.zeros(3), np.zeros(3), q, qdot, True) mass = robot.get_mass_matrix() mass_inv = np.linalg.inv(mass) g = robot.get_gravity() lambda_i = np.linalg.inv(np.dot(j_i, np.dot(mass_inv, j_i.transpose()))) j_i_bar = np.dot(np.dot(mass_inv, j_i.transpose()), lambda_i) null_i = np.eye(robot.n_q_dot) - np.dot(j_i_bar, j_i) grav_comp_cmd = np.dot( np.dot(np.linalg.pinv(np.dot(s_a, null_i).transpose()), null_i.transpose()), g) grav_comp_cmd[3] /= 2.