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, 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, 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):
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, 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, 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, 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()
class PointContact(Contact):
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 _update_jacobian(self):
self._jacobian = self._robot.get_link_jacobian(
self._link_id)[self._dim_contact:, :]
self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot(
self._link_id)
def _update_cone_constraint(self):
rot = self._robot.get_link_iso(self._link_id)[0:3, 0:3].transpose()
self._cone_constraint_mat = np.zeros((6, self._dim_contact))
self._cone_constraint_mat[0, 2] = 1.
self._cone_constraint_mat[1, 0] = 1.
self._cone_constraint_mat[1, 2] = self._mu
self._cone_constraint_mat[2, 0] = -1.
self._cone_constraint_mat[2, 2] = self._mu
self._cone_constraint_mat[3, 1] = 1.
self._cone_constraint_mat[3, 2] = self._mu
self._cone_constraint_mat[4, 1] = -1.
self._cone_constraint_mat[4, 2] = self._mu
self._cone_constraint_mat[5, 2] = -1.
self._cone_constraint_mat = np.dot(self._cone_constraint_mat, rot)
self._cone_constraint_vec = np.zeros(6)
self._cone_constraint_vec[5] = -self._rf_z_max
if self._b_data_save:
self._data_saver.add("rf_z_max_" + self._link_id, self._rf_z_max)
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):
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()
class AtlasInterface(Interface):
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 get_command(self, sensor_data):
if PnCConfig.SAVE_DATA:
self._data_saver.add('time', self._running_time)
self._data_saver.add('phase', self._control_architecture.state)
# Update State Estimator
if self._count == 0:
print("=" * 80)
print("Initialize")
print("=" * 80)
self._se.initialize(sensor_data)
self._se.update(sensor_data)
# Process Interrupt Logic
self._interrupt_logic.process_interrupts()
# Compute Cmd
command = self._control_architecture.get_command()
if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0):
self._data_saver.advance()
# Increase time variables
self._count += 1
self._running_time += PnCConfig.CONTROLLER_DT
self._sp.curr_time = self._running_time
self._sp.prev_state = self._control_architecture.prev_state
self._sp.state = self._control_architecture.state
return copy.deepcopy(command)
@property
def interrupt_logic(self):
return self._interrupt_logic
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()
class LaikagoInterface(Interface):
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 get_command(self, sensor_data):
if PnCConfig.SAVE_DATA:
self._data_saver.add('time', self._running_time)
self._data_saver.add('phase', self._control_architecture.state)
# Update State Estimator
if self._count == 0:
print("=" * 80)
print("Initialize")
print("=" * 80)
self._se.initialize(sensor_data)
self._se.update(sensor_data)
# print("total mass")
# print(self._robot.total_mass)
# print("com pos")
# print(self._robot.get_com_pos())
# Process Interrupt Logic
self._interrupt_logic.process_interrupts()
# Compute Cmd
command = self._control_architecture.get_command()
if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0):
self._data_saver.advance()
# Increase time variables
self._count += 1
self._running_time += PnCConfig.CONTROLLER_DT
self._sp.curr_time = self._running_time
self._sp.prev_state = self._control_architecture.prev_state
self._sp.state = self._control_architecture.state
return copy.deepcopy(command)
@property
def interrupt_logic(self):
return self._interrupt_logic
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.
class IHWBC(object):
"""
Implicit Hierarchy Whole Body Control
------------------
Usage:
update_setting --> solve
"""
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()
@property
def trq_limit(self):
return self._trq_limit
@property
def lambda_q_ddot(self):
return self._lambda_q_ddot
@property
def lambda_rf(self):
return self._lambda_rf
@property
def w_hierarchy(self):
return self._w_hierarchy
@property
def w_rf(self):
return self._w_rf
@trq_limit.setter
def trq_limit(self, val):
assert val.shape[0] == self._n_active
self._trq_limit = np.copy(val)
@lambda_q_ddot.setter
def lambda_q_ddot(self, val):
self._lambda_q_ddot = val
@lambda_rf.setter
def lambda_rf(self, val):
self._lambda_rf = val
@w_hierarchy.setter
def w_hierarchy(self, val):
self._w_hierarchy = val
@w_hierarchy.setter
def w_rf(self, val):
self._w_rf = val
def update_setting(self, mass_matrix, mass_matrix_inv, coriolis, gravity):
self._mass_matrix = np.copy(mass_matrix)
self._mass_matrix_inv = np.copy(mass_matrix_inv)
self._coriolis = np.copy(coriolis)
self._gravity = np.copy(gravity)
def solve(self,
task_list,
contact_list,
internal_constraint_list,
rf_des=None,
verbose=False):
"""
Parameters
----------
task_list (list of Task):
Task list
contact_list (list of Contact):
Contact list
internal_constraint_list (list of InternalConstraint):
Internal constraint list
rf_des (np.ndarray):
Reaction force desired
verbose (bool):
Printing option
Returns
-------
joint_trq_cmd (np.array):
Joint trq cmd
joint_acc_cmd (np.array):
Joint acc cmd
sol_rf (np.array):
Reaction force
"""
# ======================================================================
# Internal Constraint
# Set ni, jit_lmd_jidot_qdot, sa_ni_trc_bar_tr, and b_internal_constraint
# ======================================================================
if len(internal_constraint_list) > 0:
ji = np.concatenate(
[ic.jacobian for ic in internal_constraint_list], axis=0)
jidot_qdot = np.concatenate(
[ic.jacobian_dot_q_dot for ic in internal_constraint_list],
axis=0)
lmd = np.linalg.pinv(
np.dot(np.dot(ji, self._mass_matrix_inv), ji.transpose()))
ji_bar = np.dot(np.dot(self._mass_matrix_inv, ji.transpose()), lmd)
ni = np.eye(self._n_q_dot) - np.dot(ji_bar, ji)
jit_lmd_jidot_qdot = np.squeeze(
np.dot(np.dot(ji.transpose(), lmd), jidot_qdot))
sa_ni_trc = np.dot(self._sa, ni)[:, 6:]
sa_ni_trc_bar = util.weighted_pinv(sa_ni_trc,
self._mass_matrix_inv[6:, 6:])
sa_ni_trc_bar_tr = sa_ni_trc_bar.transpose()
b_internal_constraint = True
else:
ni = np.eye(self._n_q_dot)
jit_lmd_jidot_qdot = np.zeros(self._n_q_dot)
sa_ni_trc_bar = np.eye(self._n_active)
sa_ni_trc_bar_tr = sa_ni_trc_bar.transpose()
b_internal_constraint = False
# print("ni")
# print(ni)
# print("jit_lmd_jidot_qdot")
# print(jit_lmd_jidot_qdot)
# print("sa_ni_trc_bar_tr")
# print(sa_ni_trc_bar_tr)
# exit()
# ======================================================================
# Cost
# ======================================================================
cost_t_mat = np.zeros((self._n_q_dot, self._n_q_dot))
cost_t_vec = np.zeros(self._n_q_dot)
for i, task in enumerate(task_list):
j = task.jacobian
j_dot_q_dot = task.jacobian_dot_q_dot
x_ddot = task.op_cmd
if verbose:
print("====================")
print(task.target_id, " task")
task.debug()
cost_t_mat += self._w_hierarchy[i] * np.dot(j.transpose(), j)
cost_t_vec += self._w_hierarchy[i] * np.dot(
(j_dot_q_dot - x_ddot).transpose(), j)
# cost_t_mat += self._lambda_q_ddot * np.eye(self._n_q_dot)
cost_t_mat += self._lambda_q_ddot * self._mass_matrix
if contact_list is not None:
uf_mat = np.array(
block_diag(
*[contact.cone_constraint_mat
for contact in contact_list]))
uf_vec = np.concatenate(
[contact.cone_constraint_vec for contact in contact_list])
contact_jacobian = np.concatenate(
[contact.jacobian for contact in contact_list], axis=0)
assert uf_mat.shape[0] == uf_vec.shape[0]
assert uf_mat.shape[1] == contact_jacobian.shape[0]
dim_cone_constraint, dim_contacts = uf_mat.shape
cost_rf_mat = (self._lambda_rf + self._w_rf) * np.eye(dim_contacts)
if rf_des is None:
rf_des = np.zeros(dim_contacts)
cost_rf_vec = -self._w_rf * np.copy(rf_des)
cost_mat = np.array(block_diag(
cost_t_mat, cost_rf_mat)) # (nqdot+nc, nqdot+nc)
cost_vec = np.concatenate([cost_t_vec, cost_rf_vec]) # (nqdot+nc,)
else:
dim_contacts = dim_cone_constraint = 0
cost_mat = np.copy(cost_t_mat)
cost_vec = np.copy(cost_t_vec)
# if verbose:
# print("==================================")
# np.set_printoptions(precision=4)
# print("cost_t_mat")
# print(cost_t_mat)
# print("cost_t_vec")
# print(cost_t_vec)
# print("cost_rf_mat")
# print(cost_rf_mat)
# print("cost_rf_vec")
# print(cost_rf_vec)
# print("cost_mat")
# print(cost_mat)
# print("cost_vec")
# print(cost_vec)
# ======================================================================
# Equality Constraint
# ======================================================================
if contact_list is not None:
eq_floating_mat = np.concatenate(
(np.dot(self._sf, self._mass_matrix),
-np.dot(self._sf,
np.dot(contact_jacobian, ni).transpose())),
axis=1) # (6, nqdot+nc)
if b_internal_constraint:
eq_int_mat = np.concatenate(
(ji, np.zeros(
(ji.shape[0], dim_contacts))), axis=1) # (2, nqdot+nc)
eq_int_vec = np.zeros(ji.shape[0])
else:
eq_floating_mat = np.dot(self._sf, self._mass_matrix)
if b_internal_constraint:
eq_int_mat = np.copy(ji)
eq_int_vec = np.zeros(ji.shape[0])
eq_floating_vec = -np.dot(
self._sf, np.dot(ni.transpose(), (self._coriolis + self._gravity)))
if b_internal_constraint:
eq_mat = np.concatenate((eq_floating_mat, eq_int_mat), axis=0)
eq_vec = np.concatenate((eq_floating_vec, eq_int_vec), axis=0)
else:
eq_mat = np.copy(eq_floating_mat)
eq_vec = np.copy(eq_floating_vec)
# ======================================================================
# Inequality Constraint
# ======================================================================
if self._trq_limit is None:
if contact_list is not None:
ineq_mat = np.concatenate((np.zeros(
(dim_cone_constraint, self._n_q_dot)), -uf_mat),
axis=1)
ineq_vec = -uf_vec
else:
ineq_mat = None
ineq_vec = None
else:
if contact_list is not None:
ineq_mat = np.concatenate(
(np.concatenate(
(np.zeros((dim_cone_constraint, self._n_q_dot)),
-np.dot(sa_ni_trc_bar_tr,
np.dot(self._snf, self._mass_matrix)),
np.dot(sa_ni_trc_bar_tr,
np.dot(self._snf, self._mass_matrix))),
axis=0),
np.concatenate(
(-uf_mat,
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(contact_jacobian, ni).transpose()),
-np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(contact_jacobian, ni).transpose())),
axis=0)),
axis=1)
ineq_vec = np.concatenate(
(-uf_vec,
np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(ni.transpose(),
(self._coriolis + self._gravity))) +
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
jit_lmd_jidot_qdot) - self._trq_limit[:, 0],
-np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(ni.transpose(),
(self._coriolis + self._gravity))) -
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
jit_lmd_jidot_qdot) + self._trq_limit[:, 1]))
else:
ineq_mat = np.concatenate(
(-np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
self._mass_matrix),
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
self._mass_matrix)),
axis=0)
ineq_vec = np.concatenate(
(np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(ni.transpose(),
(self._coriolis + self._gravity))) +
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
jit_lmd_jidot_qdot) - self._trq_limit[:, 0],
-np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(ni.transpose(),
(self._coriolis + self._gravity))) -
np.dot(np.dot(sa_ni_trc_bar_tr, self._snf),
jit_lmd_jidot_qdot) + self._trq_limit[:, 1]))
# if verbose:
# print("eq_mat")
# print(eq_mat)
# print("eq_vec")
# print(eq_vec)
# print("ineq_mat")
# print(ineq_mat)
# print("ineq_vec")
# print(ineq_vec)
sol = solve_qp(cost_mat,
cost_vec,
ineq_mat,
ineq_vec,
eq_mat,
eq_vec,
solver="quadprog",
verbose=True)
if contact_list is not None:
sol_q_ddot, sol_rf = sol[:self._n_q_dot], sol[self._n_q_dot:]
else:
sol_q_ddot, sol_rf = sol, None
if contact_list is not None:
joint_trq_cmd = np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(self._mass_matrix, sol_q_ddot) +
np.dot(ni.transpose(), (self._coriolis + self._gravity)) -
np.dot(np.dot(contact_jacobian, ni).transpose(), sol_rf))
else:
joint_trq_cmd = np.dot(
np.dot(sa_ni_trc_bar_tr, self._snf),
np.dot(self._mass_matrix, sol_q_ddot) +
np.dot(ni, (self._coriolis + self._gravity)))
joint_acc_cmd = np.dot(self._sa, sol_q_ddot)
if verbose:
# if True:
print("joint_trq_cmd: ", joint_trq_cmd)
print("sol_q_ddot: ", sol_q_ddot)
print("sol_rf: ", sol_rf)
# for i, task in enumerate(task_list):
for task in [task_list[3], task_list[4]]:
j = task.jacobian
j_dot_q_dot = task.jacobian_dot_q_dot
x_ddot = task.op_cmd
print(task.target_id, " task")
print("des x ddot: ", x_ddot)
print("j*qddot_sol + Jdot*qdot: ",
np.dot(j, sol_q_ddot) + j_dot_q_dot)
if self._b_data_save:
self._data_saver.add('joint_trq_cmd', joint_trq_cmd)
self._data_saver.add('joint_acc_cmd', joint_acc_cmd)
self._data_saver.add('rf_cmd', sol_rf)
return joint_trq_cmd, joint_acc_cmd, sol_rf
class Draco3IHWBC(object):
"""
Implicit Hierarchy Whole Body Control
------------------
Usage:
update_setting --> solve
"""
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()
@property
def trq_limit(self):
return self._trq_limit
@property
def lambda_q_ddot(self):
return self._lambda_q_ddot
@property
def lambda_rf(self):
return self._lambda_rf
@property
def lambda_if(self):
return self._lambda_if
@property
def w_hierarchy(self):
return self._w_hierarchy
@trq_limit.setter
def trq_limit(self, val):
assert val.shape[0] == self._n_active
self._trq_limit = np.copy(val)
@lambda_q_ddot.setter
def lambda_q_ddot(self, val):
self._lambda_q_ddot = val
@lambda_rf.setter
def lambda_rf(self, val):
self._lambda_rf = val
@lambda_if.setter
def lambda_if(self, val):
self._lambda_if = val
@w_hierarchy.setter
def w_hierarchy(self, val):
self._w_hierarchy = val
def update_setting(self, mass_matrix, mass_matrix_inv, coriolis, gravity):
self._mass_matrix = np.copy(mass_matrix)
self._mass_matrix_inv = np.copy(mass_matrix_inv)
self._coriolis = np.copy(coriolis)
self._gravity = np.copy(gravity)
def solve(self, task_list, contact_list, verbose=False):
"""
Parameters
----------
task_list (list of Task):
Task list
contact_list (list of Contact):
Contact list
verbose (bool):
Printing option
Returns
-------
joint_trq_cmd (np.array):
Joint trq cmd
joint_acc_cmd (np.array):
Joint acc cmd
sol_rf (np.array):
Reaction force
sol_if (np.array):
Internal force
"""
# ======================================================================
# Cost
# ======================================================================
cost_t_mat = np.zeros((self._n_q_dot, self._n_q_dot))
cost_t_vec = np.zeros(self._n_q_dot)
for i, task in enumerate(task_list):
j = task.jacobian
j_dot_q_dot = task.jacobian_dot_q_dot
x_ddot = task.op_cmd
if verbose:
print(i, " th task")
task.debug()
cost_t_mat += self._w_hierarchy[i] * np.dot(j.transpose(), j)
cost_t_vec += self._w_hierarchy[i] * np.dot(
(j_dot_q_dot - x_ddot).transpose(), j)
# cost_t_mat += self._lambda_q_ddot * np.eye(self._n_q_dot)
cost_t_mat += self._lambda_q_ddot * self._mass_matrix
cost_if_mat = self._lambda_if * np.eye(self._n_int)
cost_if_vec = np.zeros(self._n_int)
if contact_list is not None:
uf_mat = np.array(
block_diag(
*[contact.cone_constraint_mat
for contact in contact_list]))
uf_vec = np.concatenate(
[contact.cone_constraint_vec for contact in contact_list])
contact_jacobian = np.concatenate(
[contact.jacobian for contact in contact_list], axis=0)
assert uf_mat.shape[0] == uf_vec.shape[0]
assert uf_mat.shape[1] == contact_jacobian.shape[0]
dim_cone_constraint, dim_contacts = uf_mat.shape
cost_rf_mat = self._lambda_rf * np.eye(dim_contacts)
cost_rf_vec = np.zeros(dim_contacts)
cost_mat = np.array(
block_diag(cost_t_mat, cost_if_mat,
cost_rf_mat)) # (nqdot+nint+nc, nqdot+nint+nc)
cost_vec = np.concatenate([cost_t_vec, cost_if_vec,
cost_rf_vec]) # (nqdot+nint+nc,)
else:
dim_contacts = dim_cone_constraint = 0
cost_mat = np.array(block_diag(cost_t_mat, cost_if_mat))
cost_vec = np.concatenate([cost_t_vec, cost_if_vec])
if verbose:
print("cost_t_mat")
print(cost_t_mat)
print("cost_t_vec")
print(cost_t_vec)
print("cost_rf_mat")
print(cost_rf_mat)
print("cost_rf_vec")
print(cost_rf_vec)
# ======================================================================
# Equality Constraint
# ======================================================================
if contact_list is not None:
eq_floating_mat = np.concatenate(
(np.dot(self._sf, self._mass_matrix), np.zeros(
(6, self._n_int)),
-np.dot(self._sf, contact_jacobian.transpose())),
axis=1) # (6, nqdot+nint+nc)
eq_int_mat = np.concatenate(
(self._jac_int, np.zeros(
(2, self._n_int)), np.zeros((2, dim_contacts))),
axis=1) # (2, nqdot+nint+nc)
else:
eq_floating_mat = np.concatenate(
(np.dot(self._sf, self._mass_matrix), np.zeros(
(6, self._n_int))),
axis=1) # (6, nqdot+nint)
eq_int_mat = np.concatenate(
(self._jac_int, np.zeros(
(2, self._n_int))), axis=1) # (2, nqdot+nint)
eq_floating_vec = -np.dot(self._sf, (self._coriolis + self._gravity))
eq_int_vec = np.zeros(2)
eq_trq_mat = np.concatenate(
(np.dot(self._jac_int, self._mass_matrix),
-np.dot(self._jac_int, self._jac_int.transpose()),
-np.dot(self._jac_int, contact_jacobian.transpose())),
axis=1)
eq_trq_vec = -np.dot(self._jac_int, (self._coriolis + self._gravity))
eq_mat = np.concatenate((eq_floating_mat, eq_trq_mat, eq_int_mat),
axis=0)
eq_vec = np.concatenate((eq_floating_vec, eq_trq_vec, eq_int_vec),
axis=0)
# ======================================================================
# Inequality Constraint
# ======================================================================
if self._trq_limit is None:
if contact_list is not None:
ineq_mat = np.concatenate((np.zeros(
(dim_cone_constraint, self._n_q_dot + self._n_int)),
-uf_mat),
axis=1)
ineq_vec = -uf_vec
else:
ineq_mat = None
ineq_vec = None
else:
if contact_list is not None:
ineq_mat = np.concatenate(
(np.concatenate((np.zeros(
(dim_cone_constraint, self._n_q_dot)),
-np.dot(self._sa, self._mass_matrix),
np.dot(self._sa, self._mass_matrix)),
axis=0),
np.concatenate(
(np.zeros((dim_cone_constraint, self._n_int)),
np.dot(self._sa, self._jac_int.transpose()),
-np.dot(self._sa, self._jac_int.transpose())),
axis=0),
np.concatenate(
(-uf_mat,
np.dot(self._sa, contact_jacobian.transpose()),
-np.dot(self._sa, contact_jacobian.transpose())),
axis=0)),
axis=1)
ineq_vec = np.concatenate(
(-uf_vec,
np.dot(self._sa, self._coriolis + self._gravity) -
self._trq_limit[:, 0],
-np.dot(self._sa, self._coriolis + self._gravity) +
self._trq_limit[:, 1]))
else:
ineq_mat = np.concatenate(
(np.concatenate((-np.dot(self._sa, self._mass_matrix),
np.dot(self._sa, self._mass_matrix)),
axis=0),
np.concatenate(
(np.dot(self._sa, self._jac_int.transpose()),
-np.dot(self._sa, self._jac_int.transpose())),
axis=0)),
axis=1)
ineq_vec = np.concatenate(
(np.dot(self._sa, self._coriolis + self._gravity) -
self._trq_limit[:, 0],
-np.dot(self._sa, self._coriolis + self._gravity) +
self._trq_limit[:, 1]))
if verbose:
print("eq_mat")
print(eq_mat)
print("eq_vec")
print(eq_vec)
print("ineq_mat")
print(ineq_mat)
print("ineq_vec")
print(ineq_vec)
sol = solve_qp(cost_mat,
cost_vec,
ineq_mat,
ineq_vec,
eq_mat,
eq_vec,
solver="quadprog",
verbose=True)
if contact_list is not None:
sol_q_ddot, sol_if, sol_rf = sol[:self._n_q_dot], sol[
self._n_q_dot:self._n_q_dot + self._n_int], sol[self._n_q_dot +
self._n_int:]
else:
sol_q_ddot, sol_if, sol_rf = sol[:self._n_q_dot], sol[
self._n_q_dot:self._n_q_dot + self._n_int], None
if contact_list is not None:
joint_trq_cmd = np.dot(
self._sa,
np.dot(self._mass_matrix, sol_q_ddot) + self._coriolis +
self._gravity - np.dot(contact_jacobian.transpose(), sol_rf) -
np.dot(self._jac_int.transpose(), sol_if))
else:
joint_trq_cmd = np.dot(
self._sa,
np.dot(self._mass_matrix, sol_q_ddot) + self._coriolis +
self._gravity - np.dot(self._jac_int.transpose(), sol_if))
joint_acc_cmd = np.dot(self._sa, sol_q_ddot)
if verbose:
print("joint_trq_cmd: ", joint_trq_cmd)
print("sol_q_ddot: ", sol_q_ddot)
print("sol_rf: ", sol_rf)
print("sol_if: ", sol_if)
if self._b_data_save:
self._data_saver.add('joint_trq_cmd', joint_trq_cmd)
self._data_saver.add('joint_acc_cmd', joint_acc_cmd)
self._data_saver.add('if_cmd', sol_if)
self._data_saver.add('rf_cmd', sol_rf)
return joint_trq_cmd, joint_acc_cmd, sol_rf, sol_if
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
class SurfaceContact(Contact):
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 _update_jacobian(self):
self._jacobian = self._robot.get_link_jacobian(self._link_id)
self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot(
self._link_id)
def _update_cone_constraint(self):
self._cone_constraint_mat = np.zeros((16 + 2, self._dim_contact))
u = self._get_u(self._x, self._y, self._mu)
rot = self._robot.get_link_iso(self._link_id)[0:3, 0:3]
rot_foot = np.zeros((6, 6))
rot_foot[0:3, 0:3] = rot.transpose()
rot_foot[3:6, 3:6] = rot.transpose()
self._cone_constraint_mat = np.dot(u, rot_foot)
self._cone_constraint_vec = np.zeros(16 + 2)
self._cone_constraint_vec[17] = -self._rf_z_max
if self._b_data_save:
self._data_saver.add("rf_z_max_" + self._link_id, self._rf_z_max)
def _get_u(self, x, y, mu):
u = np.zeros((16 + 2, 6))
u[0, 5] = 1.
u[1, 3] = 1.
u[1, 5] = mu
u[2, 3] = -1.
u[2, 5] = mu
u[3, 4] = 1.
u[3, 5] = mu
u[4, 4] = -1.
u[4, 5] = mu
u[5, 0] = 1.
u[5, 5] = y
u[6, 0] = -1.
u[6, 5] = y
u[7, 1] = 1.
u[7, 5] = x
u[8, 1] = -1.
u[8, 5] = x
##tau
u[9, 0] = -mu
u[9, 1] = -mu
u[9, 2] = 1.
u[9, 3] = y
u[9, 4] = x
u[9, 5] = (x + y) * mu
u[10, 0] = -mu
u[10, 1] = mu
u[10, 2] = 1.
u[10, 3] = y
u[10, 4] = -x
u[10, 5] = (x + y) * mu
u[11, 0] = mu
u[11, 1] = -mu
u[11, 2] = 1.
u[11, 3] = -y
u[11, 4] = x
u[11, 5] = (x + y) * mu
u[12, 0] = mu
u[12, 1] = mu
u[12, 2] = 1.
u[12, 3] = -y
u[12, 4] = -x
u[12, 5] = (x + y) * mu
u[13, 0] = -mu
u[13, 1] = -mu
u[13, 2] = -1.
u[13, 3] = -y
u[13, 4] = -x
u[13, 5] = (x + y) * mu
u[14, 0] = -mu
u[14, 1] = mu
u[14, 2] = -1.
u[14, 3] = -y
u[14, 4] = x
u[14, 5] = (x + y) * mu
u[15, 0] = mu
u[15, 1] = -mu
u[15, 2] = -1.
u[15, 3] = y
u[15, 4] = -x
u[15, 5] = (x + y) * mu
u[16, 0] = mu
u[16, 1] = mu
u[16, 2] = -1.
u[16, 3] = y
u[16, 4] = x
u[16, 5] = (x + y) * mu
u[17, 5] = -1.
return u
class BasicTask(Task):
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()
@property
def target_id(self):
return self._target_id
def update_cmd(self):
if self._task_type == "JOINT":
pos = self._robot.joint_positions
pos_err = self._pos_des - pos
vel_act = self._robot.joint_velocities
if self._b_data_save:
self._data_saver.add('joint_pos_des', self._pos_des.copy())
self._data_saver.add('joint_vel_des', self._vel_des.copy())
self._data_saver.add('joint_pos', pos.copy())
self._data_saver.add('joint_vel', vel_act.copy())
self._data_saver.add('w_joint', self._w_hierarchy)
elif self._task_type == "SELECTED_JOINT":
pos = self._robot.joint_positions[self._robot.get_joint_idx(
self._target_id)]
pos_err = self._pos_des - pos
vel_act = self._robot.joint_velocities[self._robot.get_joint_idx(
self._target_id)]
if self._b_data_save:
self._data_saver.add('joint_pos_des', self._pos_des.copy())
self._data_saver.add('joint_vel_des', self._vel_des.copy())
self._data_saver.add('joint_pos', pos.copy())
self._data_saver.add('joint_vel', vel_act.copy())
self._data_saver.add('w_joint', self._w_hierarchy)
elif self._task_type == "LINK_XYZ":
pos = self._robot.get_link_iso(self._target_id)[0:3, 3]
pos_err = self._pos_des - pos
vel_act = self._robot.get_link_vel(self._target_id)[3:6]
if self._b_data_save:
self._data_saver.add(self._target_id + '_pos_des',
self._pos_des.copy())
self._data_saver.add(self._target_id + '_vel_des',
self._vel_des.copy())
self._data_saver.add(self._target_id + '_pos', pos.copy())
self._data_saver.add(self._target_id + '_vel', vel_act.copy())
self._data_saver.add('w_' + self._target_id, self._w_hierarchy)
elif self._task_type == "LINK_ORI":
quat_des = R.from_quat(self._pos_des)
quat_act = R.from_matrix(
self._robot.get_link_iso(self._target_id)[0:3, 0:3])
quat_err = R.from_matrix(
np.dot(quat_des.as_matrix(),
quat_act.as_matrix().transpose())).as_quat()
pos_err = util.quat_to_exp(quat_err)
vel_act = self._robot.get_link_vel(self._target_id)[0:3]
if self._b_data_save:
self._data_saver.add(self._target_id + '_quat_des',
quat_des.as_quat())
self._data_saver.add(self._target_id + '_ang_vel_des',
self._vel_des.copy())
self._data_saver.add(self._target_id + '_quat',
quat_act.as_quat())
self._data_saver.add(self._target_id + '_ang_vel',
vel_act.copy())
self._data_saver.add('w_' + self._target_id + "_ori",
self._w_hierarchy)
elif self._task_type == "COM":
pos = self._robot.get_com_pos()
pos_err = self._pos_des - pos
vel_act = self._robot.get_com_lin_vel()
if self._b_data_save:
self._data_saver.add(self._target_id + '_pos_des',
self._pos_des.copy())
self._data_saver.add(self._target_id + '_vel_des',
self._vel_des.copy())
self._data_saver.add(self._target_id + '_pos', pos.copy())
self._data_saver.add(self._target_id + '_vel', vel_act.copy())
self._data_saver.add('w_' + self._target_id, self._w_hierarchy)
else:
raise ValueError
for i in range(self._dim):
self._op_cmd[i] = self._acc_des[i] + self._kp[i] * pos_err[
i] + self._kd[i] * (self._vel_des[i] - vel_act[i])
def update_jacobian(self):
if self._task_type == "JOINT":
self._jacobian[:, self._robot.n_floating:self._robot.n_floating +
self._robot.n_a] = np.eye(self._dim)
self._jacobian_dot_q_dot = np.zeros(self._dim)
elif self._task_type == "SELECTED_JOINT":
for i, jid in enumerate(self._robot.get_q_dot_idx(
self._target_id)):
self._jacobian[i, jid] = 1
self._jacobian_dot_q_dot = np.zeros(self._dim)
elif self._task_type == "LINK_XYZ":
self._jacobian = self._robot.get_link_jacobian(
self._target_id)[3:6, :]
self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot(
self._target_id)[3:6]
elif self._task_type == "LINK_ORI":
self._jacobian = self._robot.get_link_jacobian(
self._target_id)[0:3, :]
self._jacobian_dot_q_dot = self._robot.get_link_jacobian_dot_times_qdot(
self._target_id)[0:3]
elif self._task_type == "COM":
self._jacobian = self._robot.get_com_lin_jacobian()
self._jacobian_dot_q_dot = np.dot(
self._robot.get_com_lin_jacobian_dot(),
self._robot.get_q_dot())
else:
raise ValueError
class DracoManipulationController(object):
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 get_command(self):
if self._b_first_visit:
self.first_visit()
# Dynamics properties
mass_matrix = self._robot.get_mass_matrix()
mass_matrix_inv = np.linalg.inv(mass_matrix)
coriolis = self._robot.get_coriolis()
gravity = self._robot.get_gravity()
self._ihwbc.update_setting(mass_matrix, mass_matrix_inv, coriolis,
gravity)
# Task, Contact, and Internal Constraint Setup
w_hierarchy_list = []
for task in self._tci_container.task_list:
task.update_jacobian()
task.update_cmd()
w_hierarchy_list.append(task.w_hierarchy)
self._ihwbc.w_hierarchy = np.array(w_hierarchy_list)
for contact in self._tci_container.contact_list:
contact.update_contact()
for internal_constraint in self._tci_container.internal_constraint_list:
internal_constraint.update_internal_constraint()
# WBC commands
joint_trq_cmd, joint_acc_cmd, rf_cmd = self._ihwbc.solve(
self._tci_container.task_list, self._tci_container.contact_list,
self._tci_container.internal_constraint_list, None,
WBCConfig.VERBOSE)
###consider transmission constraint
# joint_trq_cmd, joint_acc_cmd, rf_cmd = self._ihwbc.solve(
# self._tci_container.task_list, self._tci_container.contact_list,
# self._tci_container.internal_constraint_list, None, True, True)
joint_trq_cmd = np.dot(self._sa[:, 6:].transpose(), joint_trq_cmd)
joint_acc_cmd = np.dot(self._sa[:, 6:].transpose(), joint_acc_cmd)
# Double integration
joint_vel_cmd, joint_pos_cmd = self._joint_integrator.integrate(
joint_acc_cmd, self._robot.joint_velocities,
self._robot.joint_positions)
if PnCConfig.SAVE_DATA:
self._data_saver.add('joint_trq_cmd', joint_trq_cmd)
command = self._robot.create_cmd_ordered_dict(joint_pos_cmd,
joint_vel_cmd,
joint_trq_cmd)
return command
def first_visit(self):
joint_pos_ini = self._robot.joint_positions
self._joint_integrator.initialize_states(np.zeros(self._robot.n_a),
joint_pos_ini)
self._b_first_visit = False
class DracoManipulationInterface(Interface):
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 get_command(self, sensor_data):
if PnCConfig.SAVE_DATA:
self._data_saver.add('time', self._running_time)
self._data_saver.add('phase', self._control_architecture.state)
# Update State Estimator
if self._count == 0:
# print("=" * 80)
# print("Initialize")
# print("=" * 80)
self._se.initialize(sensor_data)
self._se.update(sensor_data)
# Process Interrupt Logic
self._interrupt_logic.process_interrupts()
# Compute Cmd
command = self._control_architecture.get_command()
if PnCConfig.SAVE_DATA and (self._count % PnCConfig.SAVE_FREQ == 0):
self._data_saver.add('joint_pos', self._robot.joint_positions)
self._data_saver.add('joint_vel', self._robot.joint_velocities)
self._data_saver.advance()
# Increase time variables
self._count += 1
self._running_time += PnCConfig.CONTROLLER_DT
self._sp.curr_time = self._running_time
self._sp.prev_state = self._control_architecture.prev_state
self._sp.state = self._control_architecture.state
return copy.deepcopy(command)
@property
def interrupt_logic(self):
return self._interrupt_logic
