from manipulation.meshcat_cpp_utils import StartMeshcat
meshcat = StartMeshcat()
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()
# initialize context
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)
station_context = station.GetMyContextFromRoot(context)
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))
# 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)
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)
station.SetIiwaVelocity(station_context, np.zeros(7,))
def get_velocity(station, station_context):
"""
fill in your code in this method
"""
velocity_estimated = station.GetOutputPort("iiwa_velocity_estimated").Eval(station_context)
return velocity_estimated
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')
q_command = np.array([-1.57, 0.5, 0, -1.2, 0, 1.6, 0])
station.GetInputPort('iiwa_position').FixValue(station_context, q_command)
station.GetOutputPort("iiwa_position_commanded").Eval(station_context)
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))
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))