def __init__(self,
scene_graph,
draw_period=0.033333,
Tview=np.array([[1., 0., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]),
xlim=[-1., 1],
ylim=[-1, 1],
facecolor=[1, 1, 1],
use_random_colors=False,
ax=None):
default_size = matplotlib.rcParams['figure.figsize']
scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0])
figsize = (default_size[0], default_size[0]*scalefactor)
PyPlotVisualizer.__init__(self, facecolor=facecolor, figsize=figsize,
ax=ax, draw_timestep=draw_period)
self.set_name('planar_multibody_visualizer')
self._scene_graph = scene_graph
self.Tview = Tview
self.Tview_pinv = np.linalg.pinv(self.Tview)
# Pose bundle (from SceneGraph) input port.
self.DeclareAbstractInputPort("lcm_visualization",
AbstractValue.Make(PoseBundle(0)))
self.ax.axis('equal')
self.ax.axis('off')
# Achieve the desired view limits
self.ax.set_xlim(xlim)
self.ax.set_ylim(ylim)
default_size = self.fig.get_size_inches()
scalefactor = (ylim[1]-ylim[0])/(xlim[1]-xlim[0])
self.fig.set_size_inches(default_size[0],
default_size[0]*scalefactor)
# Populate body patches
self.buildViewPatches(use_random_colors)
# Populate the body fill list -- which requires doing most of
# a draw pass, but with an ax.fill() command rather than
# an in-place replacement of vertex positions to initialize
# the draw patches.
# The body fill list stores the ax patch objects in the
# order they were spawned (i.e. by body, and then by
# order of viewPatches). Drawing the tree should update them
# by iterating over bodies and patches in the same order.
self.body_fill_dict = {}
n_bodies = len(self.viewPatches.keys())
tf = np.eye(4)
for full_name in self.viewPatches.keys():
viewPatches, viewColors = self.getViewPatches(full_name, tf)
self.body_fill_dict[full_name] = []
for patch, color in zip(viewPatches, viewColors):
self.body_fill_dict[full_name] += self.ax.fill(
patch[0, :], patch[1, :], zorder=0, edgecolor='k',
facecolor=color, closed=True)
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, 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, 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, plant):
LeafSystem.__init__(self)
self.set_name("Manipulation State Machine")
self.loaded = False
self.nq = plant.num_positions()
self.plant = plant
self.robot_state_input_port = \
self._DeclareInputPort(PortDataType.kVectorValued,
plant.num_positions() +
plant.num_velocities())
self._DeclareDiscreteState(1)
self._DeclarePeriodicDiscreteUpdate(period_sec=0.01)
self.kuka_plan_output_port = \
self._DeclareAbstractOutputPort(
lambda: AbstractValue.Make(Plan()), self.CalcPlan)
self.hand_setpoint_output_port = \
self._DeclareVectorOutputPort(
BasicVector(1), self._DoCalcHandSetpointOutput)
def _DoAllocKukaSetpointOutput(self):
return AbstractValue.Make(InstantaneousKukaControllerSetpoint())
