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)
Пример #2
0
    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)
Пример #3
0
 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)
Пример #4
0
    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)
Пример #5
0
    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)
Пример #7
0
 def _DoAllocKukaSetpointOutput(self):
     return AbstractValue.Make(InstantaneousKukaControllerSetpoint())