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
def __init__(self, id, tm, hm, fm, leg_side, robot):
super(ContactTransitionEnd, self).__init__(id, robot)
self._trajectory_managers = tm
self._hierarchy_managers = hm
self._force_managers = fm
self._leg_side = leg_side
self._sp = DracoManipulationStateProvider()
self._start_time = 0.
def __init__(self, id, tm, hm, fm, robot):
super().__init__(id, robot)
self._trajectory_managers = tm
self._hierarchy_managers = hm
self._force_managers = fm
self._sp = DracoManipulationStateProvider()
self._start_time = 0.
self._trans_duration = 0.
def __init__(self, id, tm, hm, fm, robot):
super(DoubleSupportStand, self).__init__(id, robot)
self._trajectory_managers = tm
self._hierarchy_managers = hm
self._force_managers = fm
self._end_time = 0.
self._rf_z_max_time = 0.
self._com_height_des = 0.
self._start_time = 0.
self._sp = DracoManipulationStateProvider()
self._lhand_iso = np.zeros((4, 4))
self._rhand_iso = np.zeros((4, 4))
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, id, tm, hm, fm, robot):
super(DoubleSupportBalance, self).__init__(id, robot)
self._trajectory_managers = tm
self._hierarchy_managers = hm
self._force_managers = fm
self._sp = DracoManipulationStateProvider()
self._start_time = 0.
self._walking_trigger = False
self._swaying_trigger = False
self._lhand_task_trans_trigger = False
self._rhand_task_trans_trigger = False
self._lhand_task_return_trigger = False
self._rhand_task_return_trigger = False
def __init__(self, id, tm, hm, fm, robot):
super().__init__(id, robot)
self._trajectory_managers = tm
self._hierarchy_managers = hm
self._force_managers = fm
self._sp = DracoManipulationStateProvider()
self._start_time = 0.
self._moving_duration = 0.
self._trans_duration = 0.
self._rh_target_pos = np.zeros(3)
self._rh_waypoint_pos = np.zeros(3)
self._rh_target_quat = np.zeros(4)
self._lh_target_pos = np.zeros(3)
self._lh_waypoint_pos = np.zeros(3)
self._lh_target_quat = np.zeros(4)
def __init__(self, id, tm, leg_side, robot):
super(SingleSupportSwing, self).__init__(id, robot)
self._trajectory_managers = tm
self._leg_side = leg_side
self._sp = DracoManipulationStateProvider()
self._start_time = 0.
def __init__(self, robot):
super(DracoManipulationStateEstimator, self).__init__()
self._robot = robot
self._sp = DracoManipulationStateProvider(self._robot)
def __init__(self, robot):
super(DracoManipulationControlArchitecture, self).__init__(robot)
# ======================================================================
# Initialize TCIContainer
# ======================================================================
self._tci_container = DracoManipulationTCIContainer(robot)
# ======================================================================
# Initialize Controller
# ======================================================================
self._draco_manipulation_controller = DracoManipulationController(
self._tci_container, robot)
# ======================================================================
# Initialize Planner
# ======================================================================
self._dcm_planner = DCMPlanner()
# ======================================================================
# Initialize Task Manager
# ======================================================================
self._rfoot_tm = FootTrajectoryManager(
self._tci_container.rfoot_pos_task,
self._tci_container.rfoot_ori_task, robot)
self._rfoot_tm.swing_height = WalkingConfig.SWING_HEIGHT
self._lfoot_tm = FootTrajectoryManager(
self._tci_container.lfoot_pos_task,
self._tci_container.lfoot_ori_task, robot)
self._lfoot_tm.swing_height = WalkingConfig.SWING_HEIGHT
self._upper_body_tm = UpperBodyTrajectoryManager(
self._tci_container.upper_body_task, robot)
self._lhand_tm = HandTrajectoryManager(
self._tci_container.lhand_pos_task,
self._tci_container.lhand_ori_task, robot)
self._rhand_tm = HandTrajectoryManager(
self._tci_container.rhand_pos_task,
self._tci_container.rhand_ori_task, robot)
self._floating_base_tm = FloatingBaseTrajectoryManager(
self._tci_container.com_task, self._tci_container.torso_ori_task,
robot)
self._dcm_tm = DCMTrajectoryManager(self._dcm_planner,
self._tci_container.com_task,
self._tci_container.torso_ori_task,
self._robot, "l_foot_contact",
"r_foot_contact")
self._dcm_tm.nominal_com_height = WalkingConfig.COM_HEIGHT
self._dcm_tm.t_additional_init_transfer = WalkingConfig.T_ADDITIONAL_INI_TRANS
self._dcm_tm.t_contact_transition = WalkingConfig.T_CONTACT_TRANS
self._dcm_tm.t_swing = WalkingConfig.T_SWING
self._dcm_tm.percentage_settle = WalkingConfig.PERCENTAGE_SETTLE
self._dcm_tm.alpha_ds = WalkingConfig.ALPHA_DS
self._dcm_tm.nominal_footwidth = WalkingConfig.NOMINAL_FOOTWIDTH
self._dcm_tm.nominal_forward_step = WalkingConfig.NOMINAL_FORWARD_STEP
self._dcm_tm.nominal_backward_step = WalkingConfig.NOMINAL_BACKWARD_STEP
self._dcm_tm.nominal_turn_radians = WalkingConfig.NOMINAL_TURN_RADIANS
self._dcm_tm.nominal_strafe_distance = WalkingConfig.NOMINAL_STRAFE_DISTANCE
self._trajectory_managers = {
"rfoot": self._rfoot_tm,
"lfoot": self._lfoot_tm,
"upper_body": self._upper_body_tm,
"lhand": self._lhand_tm,
"rhand": self._rhand_tm,
"floating_base": self._floating_base_tm,
"dcm": self._dcm_tm
}
# ======================================================================
# Initialize Hierarchy Manager
# ======================================================================
self._rfoot_pos_hm = TaskHierarchyManager(
self._tci_container.rfoot_pos_task, WBCConfig.W_CONTACT_FOOT,
WBCConfig.W_SWING_FOOT)
self._lfoot_pos_hm = TaskHierarchyManager(
self._tci_container.lfoot_pos_task, WBCConfig.W_CONTACT_FOOT,
WBCConfig.W_SWING_FOOT)
self._rfoot_ori_hm = TaskHierarchyManager(
self._tci_container.rfoot_ori_task, WBCConfig.W_CONTACT_FOOT,
WBCConfig.W_SWING_FOOT)
self._lfoot_ori_hm = TaskHierarchyManager(
self._tci_container.lfoot_ori_task, WBCConfig.W_CONTACT_FOOT,
WBCConfig.W_SWING_FOOT)
self._lhand_pos_hm = TaskHierarchyManager(
self._tci_container.lhand_pos_task, WBCConfig.W_HAND_POS_MAX,
WBCConfig.W_HAND_POS_MIN)
self._lhand_ori_hm = TaskHierarchyManager(
self._tci_container.lhand_ori_task, WBCConfig.W_HAND_ORI_MAX,
WBCConfig.W_HAND_ORI_MIN)
self._rhand_pos_hm = TaskHierarchyManager(
self._tci_container.rhand_pos_task, WBCConfig.W_HAND_POS_MAX,
WBCConfig.W_HAND_POS_MIN)
self._rhand_ori_hm = TaskHierarchyManager(
self._tci_container.rhand_ori_task, WBCConfig.W_HAND_ORI_MAX,
WBCConfig.W_HAND_ORI_MIN)
self._hierarchy_managers = {
"rfoot_pos": self._rfoot_pos_hm,
"lfoot_pos": self._lfoot_pos_hm,
"rfoot_ori": self._rfoot_ori_hm,
"lfoot_ori": self._lfoot_ori_hm,
"lhand_pos": self._lhand_pos_hm,
"lhand_ori": self._lhand_ori_hm,
"rhand_pos": self._rhand_pos_hm,
"rhand_ori": self._rhand_ori_hm
}
# ======================================================================
# Initialize Reaction Force Manager
# ======================================================================
self._rfoot_fm = ReactionForceManager(
self._tci_container.rfoot_contact, WBCConfig.RF_Z_MAX)
self._lfoot_fm = ReactionForceManager(
self._tci_container.lfoot_contact, WBCConfig.RF_Z_MAX)
self._reaction_force_managers = {
"rfoot": self._rfoot_fm,
"lfoot": self._lfoot_fm
}
# ======================================================================
# Initialize State Machines
# ======================================================================
self._state_machine[LocomanipulationState.STAND] = DoubleSupportStand(
LocomanipulationState.STAND, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers, robot)
self._state_machine[LocomanipulationState.
STAND].end_time = WalkingConfig.INIT_STAND_DUR
self._state_machine[LocomanipulationState.
STAND].rf_z_max_time = WalkingConfig.RF_Z_MAX_TIME
self._state_machine[LocomanipulationState.
STAND].com_height_des = WalkingConfig.COM_HEIGHT
self._state_machine[
LocomanipulationState.BALANCE] = DoubleSupportBalance(
LocomanipulationState.BALANCE, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers, robot)
self._state_machine[LocomanipulationState.
LF_CONTACT_TRANS_START] = ContactTransitionStart(
LocomanipulationState.LF_CONTACT_TRANS_START,
self._trajectory_managers,
self._hierarchy_managers,
self._reaction_force_managers,
Footstep.LEFT_SIDE, self._robot)
self._state_machine[
LocomanipulationState.LF_CONTACT_TRANS_END] = ContactTransitionEnd(
LocomanipulationState.LF_CONTACT_TRANS_END,
self._trajectory_managers, self._hierarchy_managers,
self._reaction_force_managers, Footstep.LEFT_SIDE, self._robot)
self._state_machine[
LocomanipulationState.LF_SWING] = SingleSupportSwing(
LocomanipulationState.LF_SWING, self._trajectory_managers,
Footstep.LEFT_SIDE, self._robot)
self._state_machine[LocomanipulationState.
RF_CONTACT_TRANS_START] = ContactTransitionStart(
LocomanipulationState.RF_CONTACT_TRANS_START,
self._trajectory_managers,
self._hierarchy_managers,
self._reaction_force_managers,
Footstep.RIGHT_SIDE, self._robot)
self._state_machine[
LocomanipulationState.RF_CONTACT_TRANS_END] = ContactTransitionEnd(
LocomanipulationState.RF_CONTACT_TRANS_END,
self._trajectory_managers, self._hierarchy_managers,
self._reaction_force_managers, Footstep.RIGHT_SIDE,
self._robot)
self._state_machine[
LocomanipulationState.RF_SWING] = SingleSupportSwing(
LocomanipulationState.RF_SWING, self._trajectory_managers,
Footstep.RIGHT_SIDE, self._robot)
self._state_machine[
LocomanipulationState.RH_HANDREACH] = DoubleSupportHandReach(
LocomanipulationState.RH_HANDREACH, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers,
self._robot)
self._state_machine[
LocomanipulationState.
RH_HANDREACH].moving_duration = ManipulationConfig.T_REACHING_DURATION
self._state_machine[
LocomanipulationState.
RH_HANDREACH].rh_target_pos = ManipulationConfig.RH_TARGET_POS
self._state_machine[
LocomanipulationState.
RH_HANDREACH].rh_target_quat = ManipulationConfig.RH_TARGET_QUAT
self._state_machine[
LocomanipulationState.
RH_HANDREACH].trans_duration = ManipulationConfig.T_REACHING_TRANS_DURATION
self._state_machine[
LocomanipulationState.LH_HANDREACH] = DoubleSupportHandReach(
LocomanipulationState.LH_HANDREACH, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers,
self._robot)
self._state_machine[
LocomanipulationState.
LH_HANDREACH].moving_duration = ManipulationConfig.T_REACHING_DURATION
self._state_machine[
LocomanipulationState.
LH_HANDREACH].lh_target_pos = ManipulationConfig.LH_TARGET_POS
self._state_machine[
LocomanipulationState.
LH_HANDREACH].lh_target_quat = ManipulationConfig.LH_TARGET_QUAT
self._state_machine[
LocomanipulationState.
LH_HANDREACH].trans_duration = ManipulationConfig.T_REACHING_TRANS_DURATION
self._state_machine[
LocomanipulationState.RH_HANDRETURN] = DoubleSupportHandReturn(
LocomanipulationState.RH_HANDRETURN, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers,
self._robot)
self._state_machine[
LocomanipulationState.
RH_HANDRETURN].trans_duration = ManipulationConfig.T_RETURNING_TRANS_DURATION
self._state_machine[
LocomanipulationState.LH_HANDRETURN] = DoubleSupportHandReturn(
LocomanipulationState.LH_HANDRETURN, self._trajectory_managers,
self._hierarchy_managers, self._reaction_force_managers,
self._robot)
self._state_machine[
LocomanipulationState.
LH_HANDRETURN].trans_duration = ManipulationConfig.T_RETURNING_TRANS_DURATION
# Set Starting State
self._state = LocomanipulationState.STAND
self._prev_state = LocomanipulationState.STAND
self._b_state_first_visit = True
self._sp = DracoManipulationStateProvider()