<jupyter_start><jupyter_text>**Input and Output Ports of the Manipulation Station**<jupyter_code>from manipulation.meshcat_cpp_utils import StartMeshcat

meshcat = StartMeshcat()<jupyter_output><empty_output><jupyter_text>Access System Input/Output ValuesIn this exercise, you will explore the [ManipulationStation](https://drake.mit.edu/pydrake/pydrake.examples.manipulation_station.html?highlight=manipulationstationpydrake.examples.manipulation_station.ManipulationStation) that was mentioned during the lecture. The system diagram is shown below. You should recall that the orange ports are the ones that do not exist for the actual hardware platform. Now we construct a ManipulationStation object and finalize the system setting. To get a sense of what this manipulation station looks like, you can open the meshcat viewer from the generated link as usual. There should be a bookshelf and a Kuka arm with a gripper attached (it might take a few seconds to load).<jupyter_code>import numpy as np

from pydrake.examples.manipulation_station import ManipulationStation
from pydrake.systems.framework import DiagramBuilder
from pydrake.geometry import MeshcatVisualizerCpp
from pydrake.systems.analysis import Simulator

station = ManipulationStation()
station.SetupManipulationClassStation()
plant = station.get_mutable_multibody_plant()
station.Finalize()

builder = DiagramBuilder()
builder.AddSystem(station)
MeshcatVisualizerCpp.AddToBuilder(
      builder, station.GetOutputPort("query_object"), meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.Initialize()<jupyter_output><empty_output><jupyter_text>[**Context**](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=contextpydrake.systems.framework.Context_) is an abstract class template that represents all the typed values that are used in a System’s computations: time, numeric-valued input ports, numerical state, and numerical parameters. There are also type-erased abstract state variables, abstract-valued input ports, abstract parameters, and a double accuracy setting. It is important to note that a **Context** is designed to be used only with the System that created it. State and Parameter data can be copied between contexts for compatible systems as necessary. One of the most common mistakes is to pass the wrong context. Although most methods in drake should throw an error if you pass a context from the wrong system, but not all of them do yet. In the cell below, we first create a root context from the diagram, and then we retrieve the contexts of the subsystems from the root context.<jupyter_code># initialize context
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
station_context = station.GetMyContextFromRoot(context)<jupyter_output><empty_output><jupyter_text>In this exercise, you will familiarize yourself with the input and output mechanism from the manipulation station system. Remember you can always generate a schematic view of your system by running the cell below. By clicking the "+" sign on the manipulation_station, you can get a more detailed view of the diverse modules within the manipulation station.<jupyter_code>from IPython.display import HTML
from pydrake.systems.framework import GenerateHtml

diagram.set_name("diagram")
HTML('<script src="https://unpkg.com/gojs/release/go.js"></script>' + GenerateHtml(diagram))<jupyter_output><empty_output><jupyter_text>Now if we set the joint position of the Kuka arm, we should expect to get the same values from the iiwa_position_measured port, which can be found from the output ports of **manipulation_station** object. Below we demonstrate how this can be done using **drake**'s syntax. You may also find it useful to review the **system** class documentation [here](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=output_portpydrake.systems.framework.System_).<jupyter_code># provide initial states
q0 = np.array([-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
# set the joint positions of the kuka arm
station.SetIiwaPosition(station_context, q0)
# examine the output port
station.GetOutputPort("iiwa_position_measured").Eval(station_context)<jupyter_output><empty_output><jupyter_text>Note that the [output port](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=outputportpydrake.systems.framework.OutputPort) named "iiwa_position_measured" is first retrieved from the station and then evaluated using **Eval** method. This is a very common approach to read the values of a selected output port.Alternatively, you may retrieve the joint angles from the **plant**, which is a subsystem of the manipulation station.<jupyter_code>joint_angles = []
for i in range(1, 8):
  joint_angles.append(
  plant.GetJointByName('iiwa_joint_{}'.format(i)).get_angle(plant_context)
  )

# alternatively, use GetPositions to obtain the generalized positions
# from the plant context
q_general = plant.GetPositions(plant_context, 
                               plant.GetModelInstanceByName("iiwa"))

print(joint_angles)
print(q_general)<jupyter_output><empty_output><jupyter_text>Exercise a: Code SubmissionNow, it's your time to code! Use **GetOutputPort** and **Eval** to retrieve the joint velocities from the "iiwa_velocity_estimated" output port. Note that we have set the velocities for you.<jupyter_code>station.SetIiwaVelocity(station_context, np.zeros(7,))<jupyter_output><empty_output><jupyter_text>Below, `get_velocity(station, station_context)` is the function you must modify to query values from "iiwa_velocity_estimated".<jupyter_code>def get_velocity(station, station_context):
  """
  fill in your code in this method
  """
  velocity_estimated =  None
  return velocity_estimated<jupyter_output><empty_output><jupyter_text>You can check if you got the implementation correct by running the below autograder.<jupyter_code>from manipulation.exercises.robot.test_manipulation_io import TestManipulationIO
from manipulation.exercises.grader import Grader

Grader.grade_output([TestManipulationIO], [locals()], 'results.json')
Grader.print_test_results('results.json')<jupyter_output><empty_output><jupyter_text>"You may also command the Kuka joint positions via the [FixValue](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=fixvaluepydrake.systems.framework.InputPort) method. It fixes the inputs to the input port with selected values. An example is given below.<jupyter_code>q_command = np.array([-1.57, 0.5, 0, -1.2, 0, 1.6, 0])
station.GetInputPort('iiwa_position').FixValue(station_context, q_command)<jupyter_output><empty_output><jupyter_text>Now you should confirm by **GetOutputPort** and **Eval** that the "iiwa_position_commanded" port returns the same value as the q_command.<jupyter_code>station.GetOutputPort("iiwa_position_commanded").Eval(station_context)<jupyter_output><empty_output><jupyter_text>Please note that the *iiwa_position_commanded* and the *iiwa position* are NOT the same variable. The *iiwa_position_commanded* are the commanded positions sent to the robot, whereas the *iiwa_positions* are the current positions of the simulated robot. The input and output match perfectly in this case because of the luxury of simulation. However, the same observation can not be made from the feedforward torque (system input) and the commanded torque (system output). Next, you will investigate why. First, let us provide a zero feedforward torque to the "iiwa_feedforward_torque" port.<jupyter_code>station.GetInputPort("iiwa_feedforward_torque").FixValue(station_context, np.zeros(7,))
tau_no_ff = station.GetOutputPort("iiwa_torque_commanded").Eval(station_context)
print('feedforward torque: {}'.format(np.zeros(7,)))
print('commanded torque with no feedforward torque:{}'.format(tau_no_ff))<jupyter_output><empty_output><jupyter_text>Now try the same experiment with a non-zero feedforward torque as shown below.<jupyter_code>tau_ff = np.linspace(3.1, 3.7, 7)
print('feedforward torque: {}'.format(tau_ff))
station.GetInputPort("iiwa_feedforward_torque").FixValue(station_context, tau_ff)
torque_commanded = station.GetOutputPort("iiwa_torque_commanded").Eval(station_context)
print('the commanded torque: {}'.format(torque_commanded))<jupyter_output><empty_output>
{'fraction_non_alphanumeric': 0.08340374170187086, 'fraction_numerical': 0.0043452021726010865, 'mean_word_length': 6.846590909090909, 'pattern_counts': {'":': 0, '<': 55, '<?xml version=': 0, '>': 55, 'https://': 6, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'chain_length': '13', 'dir': 'jupyter-structured-clean-dedup', 'id': '332833', 'license': 'BSD-3-Clause', 'n_tokens_mistral': 2419, 'n_tokens_neox': 2330, 'n_words': 778, 'path': 'exercises/robot/manipulation_station_io.ipynb', 'repo_name': 'Magician6174/manipulation', 'size': '8285'}