def __init__(self, station, control_period=0.005, print_period=0.5):
LeafSystem.__init__(self)
self.set_name("Manipulation Plan Runner")
self.gripper_setpoint_list = []
self.kuka_plans_list = []
self.current_plan = None
# Stuff for iiwa control
self.nu = 7
self.print_period = print_period
self.last_print_time = -print_period
self.control_period = control_period
self.plant = station.get_mutable_multibody_plant()
self.tree = self.plant.tree()
# get relative transforms between EE frame (wsg gripper) and iiwa_link_7
context_plant = self.plant.CreateDefaultContext()
self.X_L7E = self.tree.CalcRelativeTransform(
context_plant,
frame_A=self.plant.GetFrameByName("iiwa_link_7"),
frame_B=self.plant.GetFrameByName("body"))
self.X_EEa = GetEndEffectorWorldAlignedFrame()
# create a multibodyplant containing the robot only, which is used for
# jacobian calculations.
self.plant_iiwa = station.get_controller_plant()
self.tree_iiwa = self.plant_iiwa.tree()
self.context_iiwa = self.plant_iiwa.CreateDefaultContext()
self.l7_frame = self.plant_iiwa.GetFrameByName('iiwa_link_7')
# Declare iiwa_position/torque_command publishing rate
self._DeclarePeriodicPublish(control_period)
# iiwa position input port
# iiwa velocity input port
self.iiwa_position_input_port = \
self._DeclareInputPort(
"iiwa_position", PortDataType.kVectorValued, 7)
self.iiwa_velocity_input_port = \
self._DeclareInputPort(
"iiwa_velocity", PortDataType.kVectorValued, 7)
# position and torque command output port
# first 7 elements are position commands.
# last 7 elements are torque commands.
self.iiwa_position_command_output_port = \
self._DeclareVectorOutputPort("iiwa_position_and_torque_command",
BasicVector(self.nu*2), self.CalcIiwaCommand)
# gripper control
self._DeclareDiscreteState(1)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.1)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self.CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self.CalcForceLimitOutput)
def __init__(self, rbt, plant, qtraj):
LeafSystem.__init__(self)
self.set_name("Manipulation State Machine")
self.qtraj = qtraj
self.rbt = rbt
self.nq = rbt.get_num_positions()
self.plant = plant
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
self._DeclareDiscreteState(1)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.001)
self.kuka_setpoint_output_port = \
self._DeclareVectorOutputPort(
BasicVector(rbt.get_num_positions() +
rbt.get_num_velocities()),
self._DoCalcKukaSetpointOutput)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(BasicVector(1),
self._DoCalcHandSetpointOutput)
self._DeclarePeriodicPublish(0.01, 0.0)
def __init__(self, plant):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self._G = plant.GetBodyByName("body").body_frame()
self._W = plant.world_frame()
self.DeclareVectorInputPort("iiwa_position", BasicVector(7))
self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7),
self.CalcOutput)
def __init__(self, plant):
LeafSystem.__init__(self)
self.plant = plant
self.plant_context = plant.CreateDefaultContext()
self.iiwa = plant.GetModelInstanceByName("iiwa7")
self.Paddle = plant.GetBodyByName("base_link")
self.W = plant.world_frame()
self.DeclareVectorInputPort("iiwa_pos_measured", BasicVector(7))
self.DeclareVectorInputPort("iiwa_velocity_estimated", BasicVector(7))
self.DeclareVectorInputPort("ball_pose", BasicVector(3))
self.DeclareVectorInputPort("ball_velocity", BasicVector(3))
self.DeclareVectorOutputPort("mirror_velocity", BasicVector(3), self.CalcOutput)
def __init__(self, plant):
LeafSystem.__init__(self)
self.plant = plant
self.plant_context = plant.CreateDefaultContext()
self.iiwa = plant.GetModelInstanceByName("iiwa7")
self.P = plant.GetBodyByName("base_link").body_frame()
self.W = plant.world_frame()
self.DeclareVectorInputPort("paddle_desired_velocity", BasicVector(3))
self.DeclareVectorInputPort("paddle_desired_angular_velocity", BasicVector(3))
self.DeclareVectorInputPort("iiwa_pos_measured", BasicVector(7))
self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7), self.CalcOutput)
self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start()
self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()
def __init__(
self,
plant,
gripper_to_object_dist,
T_world_objectPickup,
T_world_prethrow,
p_world_target,
planned_launch_angle,
height_thresh=0.03,
launch_angle_thresh=0.1,
dbg_state_prints=False # turn this to true to get some helfpul dbg prints
):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self.gripper = plant.GetBodyByName("body")
self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7))
self.qdot_port = self.DeclareVectorInputPort("iiwa_velocity", BasicVector(7))
self.DeclareVectorOutputPort("wsg_position", BasicVector(1),
self.CalcGripperTarget)
# important pose info
self.gripper_to_object_dist = gripper_to_object_dist
self.T_world_objectPickup = T_world_objectPickup
self.T_world_prethrow = T_world_prethrow
self.p_world_target = p_world_target
self.planned_launch_angle = planned_launch_angle
self.height_thresh = height_thresh
self.launch_angle_thresh = launch_angle_thresh
# some constants
self.GRIPPER_OPEN = np.array([0.5])
self.GRIPPER_CLOSED = np.array([0.0])
# states
self.gripper_picked_up_object = False
self.reached_prethrow = False
self.at_or_passed_release = False
# this helps make sure we're in the right state
self.is_robot_stationary = False
self.translation_history = np.zeros(shape=(10, 3))
self.translation_history_idx = 0
# determines gripper control based on above logic
self.should_close_gripper = False
self.dbg_state_prints = dbg_state_prints
def __init__(self, rbt_full, q_nom, world_builder):
LeafSystem.__init__(self)
self.set_name("Task Planner")
self.rbt = rbt_full
self.world_builder = world_builder
self.q_nom = np.array(
[-0.18, -1., 0.12, -1.89, 0.1, 1.3, 0.38, 0.0, 0.0, 1.5])
self.nq_full = rbt_full.get_num_positions()
self.nv_full = rbt_full.get_num_velocities()
self.nu_full = rbt_full.get_num_actuators()
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
self.nq_full + self.nv_full)
self._DeclareDiscreteState(1)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.1)
# TODO set default state somehow better. Requires new
# bindings to override AllocateDiscreteState or something else.
self.initialized = False
self.current_primitive = IdlePrimitive(self.rbt, q_nom)
self.kuka_setpoint = self._DoAllocKukaSetpointOutput()
# Put these in arrays so we can more easily pass by reference into
# CalcSetpointsOutput
self.gripper_setpoint = np.array([0.])
self.knife_setpoint = np.array([np.pi / 2.])
# TODO The controller takes full state in, even though it only
# controls the Kuka... that should be tidied up.
self.kuka_setpoint_output_port = \
self._DeclareAbstractOutputPort(
self._DoAllocKukaSetpointOutput,
self._DoCalcKukaSetpointOutput)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(BasicVector(1),
self._DoCalcHandSetpointOutput)
self.knife_setpoint_output_port = \
self._DeclareVectorOutputPort(BasicVector(1),
self._DoCalcKnifeSetpointOutput)
self._DeclarePeriodicPublish(0.01, 0.0)
# Really stupid simple state
self.current_target_object = None
self.current_target_object_move_location = None
self.do_cut_after_current_move = False
def __init__(self, plant, model_instance, control_period=0.001):
LeafSystem.__init__(self)
self.set_name("Hand Controller")
gripper_model_index_idx = 1
self.max_force = 100. # gripper max closing / opening force
self.plant = plant
self.model_instance = model_instance
# TODO: access actuation ports with model_instance index
self.nu = plant.get_input_port(gripper_model_index_idx).size()
self.nq = plant.num_positions()
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
plant.num_positions() +
plant.num_velocities())
self.setpoint_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
1)
self._DeclareDiscreteState(self.nu)
self._DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self._DoCalcVectorOutput)
def __init__(self, Q=None, R=None):
super().__init__()
self.DeclareInputPort('quadrotor state', PortDataType.kVectorValued,
12)
self.DeclareVectorOutputPort('control', BasicVector(4),
self.calculate_control)
self.m = 0.775
self.L = 0.15
self.kF = 1.0
self.kM = 0.0245
self.I = np.array([[0.0015, 0, 0], [0, 0.0025, 0], [0, 0, 0.0035]])
self.g = 9.81
self.nominal_state = [
0,
] * 12
if Q is None:
self.Q = np.diag([10, 10, 10, 20, 20, 10, 10, 10, 10, 1, 1, 1])
else:
self.Q = Q
if R is None:
self.R = np.diag([1, 1, 1, 1])
else:
self.R = R
(K, S) = self.get_LQR_params()
self.K = K
def __init__(self):
LeafSystem.__init__(self)
self._DeclareInputPort(PortDataType.kVectorValued, n)
self._DeclareVectorOutputPort(BasicVector(m), self._DoCalcVectorOutput)
self._DeclareDiscreteState(m) # state of the controller system is u
self._DeclarePeriodicDiscreteUpdate(period_sec=traj_specs.h) # update u every h seconds.
self.is_plan_computed = False
def __init__(self, rbt, plant, control_period=0.001):
LeafSystem.__init__(self)
self.set_name("GuillotineController Controller")
self.controlled_joint_names = ["knife_joint"]
self.max_force = 100.
self.controlled_inds, _ = kuka_utils.extract_position_indices(
rbt, self.controlled_joint_names)
self.nu = plant.get_input_port(2).size()
self.nq = rbt.get_num_positions()
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
self.setpoint_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
1)
self._DeclareDiscreteState(self.nu)
self._DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self._DoCalcVectorOutput)
def __init__(self, rbt, plant, control_period=0.001):
LeafSystem.__init__(self)
self.set_name("Hand Controller")
self.controlled_joint_names = [
"left_finger_sliding_joint", "right_finger_sliding_joint"
]
self.max_force = 50. # gripper max closing / opening force
self.controlled_inds, _ = kuka_utils.extract_position_indices(
rbt, self.controlled_joint_names)
self.nu = plant.get_input_port(1).size()
self.nq = rbt.get_num_positions()
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
self.setpoint_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
1)
self._DeclareDiscreteState(self.nu)
self._DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self._DoCalcVectorOutput)
def __init__(self,
builder,
dt=5e-4,
N=150,
params=None,
trj_decay=0.7,
x_w_cov=1e-5,
door_angle_ref=1.0,
visualize=False):
self.plant_derivs = MultibodyPlant(time_step=dt)
parser = Parser(self.plant_derivs)
self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
parser,
params=params)
self.plant_derivs.Finalize()
self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
self.plant_derivs.get_actuation_input_port().FixValue(
self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
self.plant_derivs_context, null_force)
self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=dt)
parser = Parser(self.plant, scene_graph)
self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
parser,
params=params)
self.plant.Finalize(
) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.
self.meshcat = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=zmq_url)
self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff
self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls
nq = self.plant.num_positions()
nv = self.plant.num_velocities()
self.n_x = nq + nv
self.n_u = self.plant.num_actuators()
self.n_y = self.plant.get_state_output_port(self.iiwa).size()
self.N = N
self.dt = dt
self.decay = trj_decay
self.V = 1e-2 * np.ones(self.n_y)
self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
self.x_w_cov = x_w_cov
self.door_angle_ref = door_angle_ref
self.q0 = np.array(
[-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
#self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
self.x0 = np.concatenate((self.q0, np.zeros(nv)))
self.door_index = None
self.phi = {}
def __init__(self):
LeafSystem.__init__(self)
self._DeclareInputPort(PortDataType.kVectorValued, n)
self._DeclareVectorOutputPort(BasicVector(m), self._DoCalcVectorOutput)
self._DeclareDiscreteState(m) # state of the controller system is u
self._DeclarePeriodicDiscreteUpdate(
period_sec=0.005) # update u at 200Hz
def __init__(self,
plant,
kuka_model_instance,
control_period=0.005,
print_period=0.5):
LeafSystem.__init__(self)
self.set_name("Kuka Controller")
self.plant = plant
self.tree = plant.tree()
self.print_period = print_period
self.last_print_time = -print_period
self.control_period = control_period
self.model_instance = kuka_model_instance
self.nu = len(get_model_actuators(plant, kuka_model_instance))
self.nq = plant.num_positions()
v_dummy = np.arange(plant.num_velocities())
self.controlled_inds = self.tree.get_velocities_from_array(
kuka_model_instance, v_dummy).astype(int)
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
plant.num_positions() +
plant.num_velocities())
self.plan_input_port = \
self._DeclareInputPort(PortDataType.kAbstractValued, 0)
self._DeclareDiscreteState(self.nu)
self._DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self._DeclareVectorOutputPort(BasicVector(self.nu),
self._DoCalcVectorOutput)
def __init__(self, plant, kuka_plans, gripper_setpoint_list):
LeafSystem.__init__(self)
self.set_name("Manipulation State Machine")
# Append plan_list with a plan that moves the robot from its current position to
# plan_list[0].traj.value(0)
kuka_plans.insert(0, JointSpacePlan())
gripper_setpoint_list.insert(0, 0.05)
self.move_to_home_duration_sec = 10.0
kuka_plans[0].duration = self.move_to_home_duration_sec
# Add a five-second zero order hold to hold the current position of the robot
kuka_plans.insert(0, JointSpacePlan())
gripper_setpoint_list.insert(0, 0.05)
self.zero_order_hold_duration_sec = 5.0
kuka_plans[0].duration = self.zero_order_hold_duration_sec
assert len(kuka_plans) == len(gripper_setpoint_list)
self.gripper_setpoint_list = gripper_setpoint_list
self.kuka_plans_list = kuka_plans
self.num_plans = len(kuka_plans)
self.t_plan = np.zeros(self.num_plans + 1)
self.t_plan[1] = self.zero_order_hold_duration_sec
self.t_plan[2] = self.move_to_home_duration_sec + self.t_plan[1]
for i in range(2, self.num_plans):
self.t_plan[i + 1] = \
self.t_plan[i] + kuka_plans[i].get_duration() * 1.1
print self.t_plan
self.nq = plant.num_positions()
self.plant = plant
self._DeclareDiscreteState(1)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.01)
self.kuka_plan_output_port = \
self._DeclareAbstractOutputPort("iiwa_plan",
lambda: AbstractValue.Make(PlanBase()), self.GetCurrentPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self.CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self.CalcForceLimitOutput)
self.iiwa_position_input_port = \
self._DeclareInputPort("iiwa_position", PortDataType.kVectorValued, 7)
def __init__(self):
LeafSystem.__init__(self)
self._DeclareInputPort(PortDataType.kVectorValued,
1,
"noise",
RandomDistribution.kGaussian)
self._DeclareContinuousState(1)
self._DeclareVectorOutputPort(BasicVector(1), self.CopyStateOut)
def __init__(self, rbt, plant,
control_period=0.033,
print_period=0.5):
LeafSystem.__init__(self)
self.set_name("Instantaneous Kuka Controller")
self.controlled_joint_names = [
"iiwa_joint_1",
"iiwa_joint_2",
"iiwa_joint_3",
"iiwa_joint_4",
"iiwa_joint_5",
"iiwa_joint_6",
"iiwa_joint_7"
]
self.controlled_inds, _ = kuka_utils.extract_position_indices(
rbt, self.controlled_joint_names)
# Extract the full-rank bit of B, and verify that it's full rank
self.nq_reduced = len(self.controlled_inds)
self.B = np.empty((self.nq_reduced, self.nq_reduced))
for k in range(self.nq_reduced):
for l in range(self.nq_reduced):
self.B[k, l] = rbt.B[self.controlled_inds[k],
self.controlled_inds[l]]
if np.linalg.matrix_rank(self.B) < self.nq_reduced:
raise RuntimeError("The joint set specified is underactuated.")
self.B_inv = np.linalg.inv(self.B)
# Copy lots of stuff
self.rbt = rbt
self.nq = rbt.get_num_positions()
self.plant = plant
self.nu = plant.get_input_port(0).size() # hopefully matches
if self.nu != self.nq_reduced:
raise RuntimeError("plant input port not equal to number of controlled joints")
self.print_period = print_period
self.last_print_time = -print_period
self.shut_up = False
self.control_period = control_period
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
self.setpoint_input_port = \
self._DeclareAbstractInputPort(
"setpoint_input_port",
AbstractValue.Make(InstantaneousKukaControllerSetpoint()))
self._DeclareDiscreteState(self.nu)
self._DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self._DeclareVectorOutputPort(
BasicVector(self.nu),
self._DoCalcVectorOutput)
def __init__(self, num_particles, mu=1.0):
LeafSystem.__init__(self)
self._DeclareInputPort(PortDataType.kVectorValued, num_particles,
RandomDistribution.kGaussian)
self._DeclareContinuousState(num_particles, num_particles, 0)
self._DeclareVectorOutputPort(BasicVector(2 * num_particles),
self.CopyStateOut)
self.num_particles = num_particles
self.mu = mu
def __init__(self, plant):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self._G = plant.GetBodyByName("body").body_frame()
self._W = plant.world_frame()
self.w_G_port = self.DeclareVectorInputPort("omega_WG", BasicVector(3))
self.v_G_port = self.DeclareVectorInputPort("v_WG", BasicVector(3))
self.q_port = self.DeclareVectorInputPort("iiwa_position",
BasicVector(7))
self.DeclareVectorOutputPort("iiwa_velocity", BasicVector(7),
self.CalcOutput)
# TODO(russt): Add missing binding
#joint_indices = plant.GetJointIndices(self._iiwa)
#self.position_start = plant.get_joint(joint_indices[0]).position_start()
#self.position_end = plant.get_joint(joint_indices[-1]).position_start()
self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start()
self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()
def __init__(self, kuka_plans, gripper_setpoint_list, update_period=0.05):
LeafSystem.__init__(self)
self.set_name("Plan Scheduler")
assert len(kuka_plans) == len(gripper_setpoint_list)
# Add a zero order hold to hold the current position of the robot
kuka_plans.insert(
0, JointSpacePlanRelative(duration=3.0, delta_q=np.zeros(7)))
gripper_setpoint_list.insert(0, 0.055)
if len(kuka_plans) > 1:
# Insert to the beginning of plan_list a plan that moves the robot from its
# current position to plan_list[0].traj.value(0)
kuka_plans.insert(
1,
JointSpacePlanGoToTarget(
duration=6.0,
q_target=kuka_plans[1].traj.value(0).flatten()))
gripper_setpoint_list.insert(0, 0.055)
self.gripper_setpoint_list = gripper_setpoint_list
self.kuka_plans_list = kuka_plans
self.current_plan = None
self.current_gripper_setpoint = None
self.current_plan_idx = 0
# Output ports for plans and gripper setpoints
self.iiwa_plan_output_port = self._DeclareAbstractOutputPort(
"iiwa_plan", lambda: AbstractValue.Make(PlanBase()),
self._GetCurrentPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self._CalcForceLimitOutput)
self._DeclarePeriodicPublish(update_period)
def __init__(self, update_period=0.05):
LeafSystem.__init__(self)
self.set_name("Plan Scheduler")
self.current_plan = None
self.current_gripper_setpoint = None
self.end_time = 0
# Output ports for plans and gripper setpoints
self.iiwa_plan_output_port = self._DeclareAbstractOutputPort(
"iiwa_plan", lambda: AbstractValue.Make(PlanBase()),
self._GetCurrentPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
"gripper_setpoint", BasicVector(1), self._CalcHandSetpointOutput)
self.gripper_force_limit_output_port = \
self._DeclareVectorOutputPort(
"force_limit", BasicVector(1), self._CalcForceLimitOutput)
self._DeclarePeriodicPublish(update_period)
def __init__(
self,
plant,
T_world_objectPickup,
T_world_prethrow,
T_world_targetRelease,
dbg_state_prints=False # turn this to true to get some helfpul dbg prints
):
LeafSystem.__init__(self)
self._plant = plant
self._plant_context = plant.CreateDefaultContext()
self._iiwa = plant.GetModelInstanceByName("iiwa")
self.gripper = plant.GetBodyByName("body")
self.q_port = self.DeclareVectorInputPort("iiwa_position", BasicVector(7))
self.qdot_port = self.DeclareVectorInputPort("iiwa_velocity", BasicVector(7))
self.DeclareVectorOutputPort("wsg_position", BasicVector(1),
self.CalcGripperTarget)
# important poses
self.T_world_objectPickup = T_world_objectPickup
self.T_world_prethrow = T_world_prethrow
self.T_world_targetRelease = T_world_targetRelease
# some constants
self.GRIPPER_OPEN = np.array([0.5])
self.GRIPPER_CLOSED = np.array([0.0])
# states
self.gripper_picked_up_object = False
self.reached_prethrow = False
self.at_or_passed_release = False
# this helps make sure we're in the right state
self.is_robot_stationary = False
self.pose_translation_history = collections.deque(maxlen=10)
# determines gripper control based on above logic
self.should_close_gripper = False
self.dbg_state_prints = dbg_state_prints
def __init__(self, feedback_rule, control_period=0.0333, print_period=1.0):
LeafSystem.__init__(self)
self.feedback_rule = feedback_rule
self.print_period = print_period
self.control_period = control_period
self.last_print_time = -print_period
self.DeclareInputPort(PortDataType.kVectorValued, 10)
self.DeclareDiscreteState(2)
self.DeclarePeriodicDiscreteUpdate(period_sec=control_period)
self.DeclareVectorOutputPort(BasicVector(2), self.DoCalcVectorOutput)
def __init__(self,
hand,
x_nom,
num_fingers,
manipuland_trajectory_callback=None,
manipuland_link_name="manipuland_body",
finger_link_name="link_3",
mu=0.5,
n_grasp_search_iters=200,
control_period=0.0333,
print_period=1.0):
LeafSystem.__init__(self)
# Copy lots of stuff
self.hand = hand
self.x_nom = x_nom
self.nq = hand.get_num_positions()
self.nu = hand.get_num_actuators()
self.manipuland_link_name = manipuland_link_name
self.manipuland_trajectory_callback = manipuland_trajectory_callback
self.n_grasp_search_iters = n_grasp_search_iters
self.mu = mu
self.num_fingers = num_fingers
self.num_finger_links = (self.nq - 3) / num_fingers
self.fingertip_position = np.array([1.2, 0., 0.])
self.print_period = print_period
self.last_print_time = -print_period
self.shut_up = False
self.DeclareInputPort(
PortDataType.kVectorValued,
hand.get_num_positions() + hand.get_num_velocities())
self.DeclareDiscreteState(self.nu)
self.DeclarePeriodicDiscreteUpdate(period_sec=control_period)
hand_actuators = hand.get_num_actuators()
self.finger_link_indices = []
# The hand plant wants every finger input as a
# separate vector. Rather than using a mux down the
# road, we'll just output in the format it wants.
for i in range(num_fingers):
self.DeclareVectorOutputPort(
BasicVector(hand_actuators / num_fingers),
functools.partial(self.DoCalcVectorOutput, i))
self.finger_link_indices.append(
self.hand.FindBody("link_3", model_id=i).get_body_index())
# Generate manipuland planar geometry and sample grasps.
self.PlanGraspPoints()
def BalancingLQR(robot):
# Design an LQR controller for stabilizing the CartPole around the upright.
# Returns a (static) AffineSystem that implements the controller (in
# the original CartPole coordinates).
context = robot.CreateDefaultContext()
context.FixInputPort(0, BasicVector([0]))
context.get_mutable_continuous_state_vector().SetFromVector(UprightState())
Q = np.diag((10., 10., 1., 1.))
R = [1]
return LinearQuadraticRegulator(robot, context, Q, R)
def ports_init(self, context):
self.plant_context = self.plant.GetMyMutableContextFromRoot(context)
self.plant.SetPositionsAndVelocities(self.plant_context, self.x0)
#door_angle = self.plant.GetPositionsFromArray(self.hinge, self.x0[:8])
self.door_index = self.plant.GetJointByName(
'right_door_hinge').position_start()
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant.GetInputPort("applied_generalized_force").FixValue(
self.plant_context, null_force)
self.plant.get_actuation_input_port().FixValue(
self.plant_context, [0., 0., 0., 0., 0., 0., 0.])
self.W0[self.door_index] = self.x_w_cov
def __init__(self,
m1=1.,
l1=1.,
m2=2.,
l2=2.,
r=1.0,
g=10.,
input_max=10.):
#VectorSystem.__init__(self,
# 1, # One input (torque at reaction wheel).
# 4) # Four outputs (theta, phi, dtheta, dphi)
LeafSystem.__init__(self)
# One inputs
self.DeclareVectorInputPort("u", BasicVector(1))
# Four outputs (full state)
self.DeclareVectorOutputPort("y",
BasicVector(4),
self.CopyStateOut,
prerequisites_of_calc=set([
self.all_state_ticket()
])) #self.CopyStateOut,
self.DeclareContinuousState(
4) # Four states (theta, phi, dtheta, dphi).
self.m1 = float(m1)
self.l1 = float(l1)
self.m2 = float(m2)
self.l2 = float(l2)
self.r = float(r)
self.g = float(g)
self.input_max = float(input_max)
# Go ahead and calculate rotational inertias.
# Treat the first link as a point mass.
self.I1 = self.m1 * self.l1**2
# Treat the second link as a disk.
self.I2 = 0.5 * self.m2 * self.r**2
def pwa_from_RigidBodyPlant(plant, linearization_points, X, U, h, method='zero_order_hold'):
"""
Arguments
----------
plant : RigidBodyPlant
RigidBodyPlant of the robot.
linearization_points : list of numpy.ndarray
Points in the state space where to linearize the dynamics.
X : Polyhedron
Overall bounds of the state space.
U : Polyhedron
Overall bounds of the control space.
h : float
Sampling time for the time discretization of the PWA system.
method : string
Method used to discretize each piece of the PWA dynamics ('explicit_euler' or 'zero_order_hold').
Returns
----------
PWA : PieceWiseAffineSystem
"""
# partition of the state space
X_partition = constrained_voronoi(linearization_points, X)
domains = [Xi.cartesian_product(U) for Xi in X_partition]
# create context
context = plant.CreateDefaultContext()
state = context.get_mutable_continuous_state_vector()
input = BasicVector(np.array([0.]))
context.FixInputPort(0, input)
# affine systems
affine_systems = []
for x in linearization_points:
state.SetFromVector(x)
taylor_approx = FirstOrderTaylorApproximation(plant, context)
affine_system = AffineSystem.from_continuous(
taylor_approx.A(),
taylor_approx.B(),
taylor_approx.f0(),
h,
method
)
affine_systems.append(affine_system)
return PieceWiseAffineSystem(affine_systems, domains)
def __init__(self, plant, traj_p_G, traj_R_G, traj_wsg_command):
LeafSystem.__init__(self)
self.plant = plant
self.gripper_body = plant.GetBodyByName("body")
self.left_finger_joint = plant.GetJointByName(
"left_finger_sliding_joint")
self.right_finger_joint = plant.GetJointByName(
"right_finger_sliding_joint")
self.traj_p_G = traj_p_G
self.traj_R_G = traj_R_G
self.traj_wsg_command = traj_wsg_command
self.plant_context = plant.CreateDefaultContext()
self.DeclareVectorOutputPort("position",
BasicVector(plant.num_positions()),
self.CalcPositionOutput)