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