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): 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(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()
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) nq, nv, na, joint_id, link_id, pos_basejoint_to_basecom, rot_basejoint_to_basecom = pybullet_util.get_robot_config( robot, SimConfig.INITIAL_POS_WORLD_TO_BASEJOINT, SimConfig.INITIAL_QUAT_WORLD_TO_BASEJOINT, SimConfig.PRINT_ROBOT_INFO) # Initial Config set_initial_config(robot, joint_id) # Joint Friction pybullet_util.set_joint_friction(robot, joint_id, 2) # RobotSystem if args.dyn_lib == 'dart': from pnc.robot_system.dart_robot_system import DartRobotSystem robot_sys = DartRobotSystem( cwd + "/robot_model/atlas/atlas_v4_with_multisense_relative_path.urdf", False, True) elif args.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 # Run Sim t = 0 dt = SimConfig.CONTROLLER_DT count = 0 step = 0
for ee in range(2): joint_screws_in_ee_at_home[ee], ee_SE3_at_home[ ee] = pybullet_util.get_kinematics_config(robot, joint_id, link_id, open_chain_joints[ee], base_link[ee], ee_link[ee]) # Initial Config set_initial_config(robot, joint_id) # 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/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
import os import sys cwd = os.getcwd() sys.path.append(cwd) import math import numpy as np import pinocchio as pin from pnc.robot_system.dart_robot_system import DartRobotSystem from pnc.robot_system.pinocchio_robot_system import PinocchioRobotSystem urdf_file = cwd + '/robot_model/manipulator/two_link_manipulator.urdf' package_dir = cwd + '/robot_model/manipulator' pin_robot_sys = PinocchioRobotSystem(urdf_file, package_dir, True, True) dart_robot_sys = DartRobotSystem(urdf_file, True, True) def analytic_jacobian(q): J = np.zeros((6, 2)) J[3, 0] = -np.sin(q[0]) - np.sin(q[0] + q[1]) J[3, 1] = -np.sin(q[0] + q[1]) J[4, 0] = np.cos(q[0]) + np.cos(q[0] + q[1]) J[4, 1] = np.cos(q[0] + q[1]) J[2, 0] = 1. J[2, 1] = 1. return J