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 __init__(self): super(Draco3LBInterface, self).__init__() if PnCConfig.DYN_LIB == "dart": from pnc.robot_system.dart_robot_system import DartRobotSystem self._robot = DartRobotSystem( cwd + "/robot_model/draco3/draco3_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/draco3/draco3_lb.urdf", cwd + "/robot_model/draco3", False, PnCConfig.PRINT_ROBOT_INFO) else: raise ValueError("wrong dynamics library") self._sp = Draco3LBStateProvider(self._robot) self._se = Draco3LBStateEstimator(self._robot) self._control_architecture = Draco3LBControlArchitecture(self._robot) self._interrupt_logic = Draco3LBInterruptLogic( 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 __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 __init__(self, act_list, data_save=False): 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) self._trq_limit = None self._lambda_q_ddot = 0. self._lambda_rf = 0. self._w_hierarchy = 0. self._b_data_save = data_save if self._b_data_save: self._data_saver = DataSaver()
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 __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()
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 __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 __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()
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 __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 __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()
pybullet_util.set_joint_friction(robot, joint_id, 0) if DYN_LIB == 'dart': from pnc.robot_system.dart_robot_system import DartRobotSystem robot_sys = DartRobotSystem( cwd + "/robot_model/atlas/atlas_rel_path.urdf", False, True) elif DYN_LIB == 'pinocchio': from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem robot_sys = PinocchioRobotSystem(cwd + "/robot_model/atlas/atlas.urdf", cwd + "/robot_model/atlas", False, True) else: raise ValueError # DataSaver data_saver = DataSaver("atlas_crbi_validation.pkl") # Run Sim t = 0 dt = DT count = 0 nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) nominal_lf_iso = pybullet_util.get_link_iso(robot, link_id['l_sole']) nominal_rf_iso = pybullet_util.get_link_iso(robot, link_id['r_sole']) base_pos = np.copy(nominal_sensor_data['base_com_pos']) base_quat = np.copy(nominal_sensor_data['base_com_quat']) joint_pos = copy.deepcopy(nominal_sensor_data['joint_pos']) s = 0.
# Joint Friction pybullet_util.set_joint_friction(robot, joint_id, 0) if DYN_LIB == 'dart': from pnc.robot_system.dart_robot_system import DartRobotSystem robot_sys = DartRobotSystem(cwd + "/robot_model/nao/nao_rel_path.urdf", False, True) elif DYN_LIB == 'pinocchio': from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem robot_sys = PinocchioRobotSystem(cwd + "/robot_model/nao/nao.urdf", cwd + "/robot_model/nao", False, True) else: raise ValueError # DataSaver data_saver = DataSaver("nao_crbi_validation.pkl") # Run Sim t = 0 dt = DT count = 0 nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) nominal_lf_iso = pybullet_util.get_link_iso(robot, link_id['l_sole']) nominal_rf_iso = pybullet_util.get_link_iso(robot, link_id['r_sole']) base_pos = np.copy(nominal_sensor_data['base_com_pos']) base_quat = np.copy(nominal_sensor_data['base_com_quat']) joint_pos = copy.deepcopy(nominal_sensor_data['joint_pos']) s = 0.
pybullet_util.set_joint_friction(robot, joint_id, 0) if DYN_LIB == 'dart': from pnc.robot_system.dart_robot_system import DartRobotSystem robot_sys = DartRobotSystem( cwd + "/robot_model/valkyrie/valkyrie_rel_path.urdf", False, True) elif DYN_LIB == 'pinocchio': from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem robot_sys = PinocchioRobotSystem( cwd + "/robot_model/valkyrie/valkyrie.urdf", cwd + "/robot_model/valkyrie", False, True) else: raise ValueError # DataSaver data_saver = DataSaver("valkyrie_crbi_validation.pkl") # Run Sim t = 0 dt = DT count = 0 nominal_sensor_data = pybullet_util.get_sensor_data( robot, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom) nominal_lf_iso = pybullet_util.get_link_iso(robot, link_id['leftCOP_Frame']) nominal_rf_iso = pybullet_util.get_link_iso(robot, link_id['rightCOP_Frame']) base_pos = np.copy(nominal_sensor_data['base_com_pos']) base_quat = np.copy(nominal_sensor_data['base_com_quat'])