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(ManipulatorInterface, self).__init__()
self._robot = PinocchioRobotSystem(
cwd + "/robot_model/manipulator/three_link_manipulator.urdf",
cwd + "/robot_model/manipulator", True,
ManipulatorConfig.PRINT_ROBOT_INFO)
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):
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)
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
nominal_sensor_data = pybullet_util.get_sensor_data(
robot, joint_id, link_id, pos_basejoint_to_basecom,
rot_basejoint_to_basecom)
while (1):
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
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/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'])
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
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(fixedTimeStep=DT, numSubSteps=1)
# Create Robot, Ground
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(
cwd + '/robot_model/manipulator/two_link_manipulator_rolling.urdf',
basePosition=[0., 0., 0.],
baseOrientation=[0., 0., 0., 1.],
useFixedBase=True)
# for i in range(3):
# p.changeDynamics(0, i, localInertiaDiagonal=[1., 1., 1.])
pin_robot = PinocchioRobotSystem(
cwd + '/robot_model/manipulator/two_link_manipulator_rolling.urdf',
cwd + '/obot_model/manipulator', True, True)
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, [0., 0., 0.], [0., 0., 0., 1.], True)
# Initial Config
# p.resetJointState(robot, joint_id["j0"], np.pi / 6, 0.)
# p.resetJointState(robot, joint_id["j1"], np.pi / 6, 0.)
p.resetJointState(robot, joint_id["j0"], 0, 0.)
p.resetJointState(robot, joint_id["j1"], 0, 0.)
# Link Damping
pybullet_util.set_link_damping(robot, link_id.values(), 0., 0.)
cameraPitch=0,
cameraTargetPosition=[0., 0., -1.5])
p.setGravity(0, 0, -9.81)
p.setPhysicsEngineParameter(fixedTimeStep=DT, numSubSteps=1)
# Create Robot, Ground
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
robot = p.loadURDF(
'/home/apptronik/catkin_ws/src/draco_3/models/draco_3_model/urdf/rolling_joint2.urdf',
useFixedBase=True)
# for i in range(3):
# p.changeDynamics(0, i, localInertiaDiagonal=[1., 1., 1.])
pin_robot = PinocchioRobotSystem(
'/home/apptronik/catkin_ws/src/draco_3/models/draco_3_model/urdf/rolling_joint2.urdf',
'/home/apptronik/catkin_ws/src/draco_3/models/draco_3_model/urdf',
True, True)
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, [0., 0., 0.], [0., 0., 0., 1.], True)
# Initial Config
p.resetJointState(robot, joint_id["jb00"], -np.pi / 6, 0.)
p.resetJointState(robot, joint_id["jb0"], np.pi / 6, 0.)
p.resetJointState(robot, joint_id["jb1"], np.pi / 6, 0.)
p.resetJointState(robot, joint_id["jp"], np.pi / 6, 0.)
p.resetJointState(robot, joint_id["jd"], np.pi / 6, 0.)
p.resetJointState(robot, joint_id["ja1"], np.pi / 4, 0.)
p.resetJointState(robot, joint_id["ja2"], -np.pi / 4, 0.)
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/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)
from pinocchio.visualize import MeshcatVisualizer
import pinocchio as pin
import cv2
import pybullet as p
import numpy as np
np.set_printoptions(precision=5)
from util import pybullet_util
from util import util
DT = 0.001
pin_robot = PinocchioRobotSystem(
'/home/apptronik/Repository/PnC/robot_model/draco/draco.urdf',
'/home/apptronik/Repository/PnC/robot_model/draco', True, True)
l_jp_idx, l_jd_idx, r_jp_idx, r_jd_idx = pin_robot.get_q_dot_idx(
['l_knee_fe_jp', 'l_knee_fe_jd', 'r_knee_fe_jp', 'r_knee_fe_jd'])
act_list = [False] * pin_robot.n_floating + [True] * pin_robot.n_a
act_list[l_jd_idx] = False
act_list[r_jd_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
s_a = np.zeros((n_active, n_q_dot))
j, k = 0, 0
for i in range(n_q_dot):
if act_list[i]:
s_a[j, i] = 1.
