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, 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, 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, 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, draw_timestep=0.033333, facecolor=[1, 1, 1],
figsize=None, ax=None):
LeafSystem.__init__(self)
self.set_name('pyplot_visualization')
self.timestep = draw_timestep
self.DeclarePeriodicPublish(draw_timestep, 0.0)
if ax is None:
(self.fig, self.ax) = plt.subplots(facecolor=facecolor,
figsize=figsize)
else:
self.ax = ax
self.fig = ax.get_figure()
self.ax.axis('equal')
self.ax.axis('off')
self.record = False
self.recorded_contexts = []
self.show = (matplotlib.get_backend().lower() != 'template')
def on_initialize(context, event):
if self.show:
self.fig.show()
self.DeclareInitializationEvent(
event=PublishEvent(
trigger_type=TriggerType.kInitialization,
callback=on_initialize))
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,
draw_timestep=0.033333,
facecolor=[1, 1, 1],
figsize=None,
ax=None):
LeafSystem.__init__(self)
self.set_name('pyplot_visualization')
self.timestep = draw_timestep
self.DeclarePeriodicPublish(draw_timestep, 0.0)
if ax is None:
(self.fig, self.ax) = plt.subplots(facecolor=facecolor,
figsize=figsize)
else:
self.ax = ax
self.fig = ax.get_figure()
self.ax.axis('equal')
self.ax.axis('off')
self.record = False
self.recorded_contexts = []
self.show = (matplotlib.get_backend().lower() != 'template')
def on_initialize(context, event):
if self.show:
self.fig.show()
self.DeclareInitializationEvent(event=PublishEvent(
trigger_type=TriggerType.kInitialization, callback=on_initialize))
def __init__(self,
rbtree,
old_mrbv=None,
draw_timestep=0.033333,
prefix="RBViz",
zmq_url="tcp://127.0.0.1:6000",
draw_collision=False,
clear_vis=False):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
self.rbtree = rbtree
self.draw_collision = draw_collision
self._DeclareInputPort(
PortDataType.kVectorValued,
self.rbtree.get_num_positions() + self.rbtree.get_num_velocities())
# Set up meshcat
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.old_mrbv = old_mrbv
# Don't both with caching logic if prefixes are different
if self.old_mrbv is not None and self.old_mrbv.prefix != self.prefix:
raise ValueError("Can't do any caching since old_mrbv has ",
"different prefix than current mrbv.")
self.body_names = []
if clear_vis:
self.vis.delete()
# Publish the tree geometry to get the visualizer started
self.PublishAllGeometry()
def __init__(self,
camera,
rbt,
draw_timestep=0.033333,
prefix="RBCameraViz",
zmq_url="tcp://127.0.0.1:6000"):
LeafSystem.__init__(self)
self.set_name('camera meshcat visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
self.camera = camera
self.rbt = rbt
self.prefix = prefix
self.camera_input_port = \
self._DeclareAbstractInputPort(
"depth_im",
AbstractValue.Make(Image[PixelType.kDepth32F](
self.camera.depth_camera_info().width(),
self.camera.depth_camera_info().height())))
self.state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
# Set up meshcat
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[prefix].delete()
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):
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):
LeafSystem.__init__(self)
self.DeclareContinuousState(1, 1, 0)
self.mu = 0.2
self.last_poincare = None # placeholder for writing return map result.
self.poincare_witness = self.MakeWitnessFunction(
"poincare", WitnessFunctionDirection.kNegativeThenNonNegative,
self.poincare, UnrestrictedUpdateEvent(self.record_poincare))
def __init__(self, body_index):
LeafSystem.__init__(self)
self.body_index = body_index
self.DeclareAbstractInputPort(
"poses",
plant.get_body_poses_output_port().Allocate())
self.DeclareAbstractOutputPort(
"pose",
lambda: AbstractValue.Make(RigidTransform()),
self.CalcOutput)
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, num_particles, mu=1.0):
LeafSystem.__init__(self)
self.DeclareInputPort("noise",
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, 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, 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, hand_controller, hand_plant):
LeafSystem.__init__(self)
self.hand_controller = hand_controller
self.hand_plant = hand_plant
self.n_cf = len(hand_controller.grasp_points)
self._data = []
self._sample_times = np.empty((0, 1))
self.shut_up = False
# Contact results
# self.DeclareInputPort('contact_results', PortDataType.kAbstractValued,
# hand_plant.contact_results_output_port().size())
self.DeclareAbstractInputPort("contact_results",
AbstractValue.Make(mut.ContactResults()))
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,
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 __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,
draw_timestep=0.033333,
facecolor=[1, 1, 1],
figsize=None):
LeafSystem.__init__(self)
self.set_name('pyplot_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
(self.fig, self.ax) = plt.subplots(facecolor=facecolor,
figsize=figsize)
self.ax.axis('equal')
self.ax.axis('off')
self.fig.show()
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, 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)
def __init__(self, mbp, q_des):
LeafSystem.__init__(self)
self.set_name('DecayingForceToDesiredConfigSystem')
self.robot_state_input_port = self.DeclareVectorInputPort(
"robot_state",
BasicVector(mbp.num_positions() + mbp.num_velocities()))
forces_cls = Value[DrakeBindingList[ExternallyAppliedSpatialForce]]
self.spatial_forces_output_port = self.DeclareAbstractOutputPort(
"spatial_forces_vector", lambda: forces_cls(),
self.DoCalcAbstractOutput)
self.mbp = mbp
self.q_des = q_des
self.mbp_current_context = mbp.CreateDefaultContext()
self.mbp_des_context = mbp.CreateDefaultContext()
self.mbp.SetPositions(self.mbp_des_context, self.q_des)
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,
rbt,
rbp,
cutting_body_index,
cut_direction,
cut_normal,
min_cut_force,
cuttable_body_indices,
timestep=0.01,
name="Cutting Guard",
last_cut_time=0.):
''' Watches the RBT contact results output, and
raises an exception (to pause simulation). '''
LeafSystem.__init__(self)
self.set_name(name)
self._DeclarePeriodicPublish(timestep, 0.0)
self.rbt = rbt
self.rbp = rbp
self.last_cut_time = last_cut_time
self.collision_id_to_body_index_map = {}
for k in range(self.rbt.get_num_bodies()):
for i in rbt.get_body(k).get_collision_element_ids():
self.collision_id_to_body_index_map[i] = k
self.cutting_body_index = cutting_body_index
self.cutting_body_ids = rbt.get_body(
cutting_body_index).get_collision_element_ids()
self.cut_direction = np.array(cut_direction)
self.cut_direction /= np.linalg.norm(self.cut_direction)
self.cut_normal = np.array(cut_normal)
self.cut_normal /= np.linalg.norm(self.cut_normal)
self.min_cut_force = min_cut_force
self.cuttable_body_indices = cuttable_body_indices
self.state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
rbt.get_num_positions() +
rbt.get_num_velocities())
self.contact_results_input_port = \
self._DeclareAbstractInputPort(
"contact_results_input_port",
AbstractValue.Make(ContactResults()))
def __init__(self,
scene_graph,
draw_timestep=0.033333,
prefix="SceneGraph",
zmq_url="tcp://127.0.0.1:6000"):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
# Pose bundle (from SceneGraph) input port.
self._DeclareInputPort(PortDataType.kAbstractValued, 0)
# Set up meshcat.
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[self.prefix].delete()
self._scene_graph = scene_graph
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,
rbtree,
draw_timestep=0.033333,
tree_prefix="RBViz",
zmq_url="tcp://127.0.0.1:6000"):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self._DeclarePeriodicPublish(draw_timestep, 0.0)
self.rbtree = rbtree
self._DeclareInputPort(
PortDataType.kVectorValued,
self.rbtree.get_num_positions() + self.rbtree.get_num_velocities())
# Set up meshcat
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis.delete()
# Publish the tree geometry to get the visualizer started
self.PublishAllGeometry()
def __init__(self,
rbtree,
draw_timestep=0.033333,
prefix="RBViz",
zmq_url="tcp://127.0.0.1:6000",
draw_collision=False):
LeafSystem.__init__(self)
self.set_name('meshcat_visualization')
self.timestep = draw_timestep
self.DeclarePeriodicPublish(draw_timestep, 0.0)
self.rbtree = rbtree
self.draw_collision = draw_collision
self.DeclareInputPort(PortDataType.kVectorValued,
self.rbtree.get_num_positions() +
self.rbtree.get_num_velocities())
# Set up meshcat
self.prefix = prefix
self.vis = meshcat.Visualizer(zmq_url=zmq_url)
self.vis[self.prefix].delete()
# Publish the tree geometry to get the visualizer started
self.PublishAllGeometry()