from manipulation.meshcat_cpp_utils import StartMeshcat
meshcat = StartMeshcat()
Meshcat is now available at https://d6fba225-7ab2-4b5a-9755-613336095cbb.deepnoteproject.com
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)
[-1.57, 0.1, 0.0, -1.2, 0.0, 1.6, 0.0]
[-1.57 0.1 0. -1.2 0. 1.6 0. ]
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')
Total score is 5/5.
Score for Test get_velocity implementation is 5/5.
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))
feedforward torque: [0. 0. 0. 0. 0. 0. 0.]
commanded torque with no feedforward torque:[-13.59469518 83.99514279 -13.07262911 -34.27181406 -13.01727483
13.33800207 0.19496751]
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))
feedforward torque: [3.1 3.2 3.3 3.4 3.5 3.6 3.7]
the commanded torque: [-10.49469518 87.19514279 -9.77262911 -30.87181406 -9.51727483
16.93800207 3.89496751]
::_::manipulation_station Context (of a Diagram)
-------------------------------------------------
7 total continuous states
40 total discrete states in 7 groups
531 total numeric parameters in 64 groups
34 total abstract parameters
::_::manipulation_station::plant Context
-----------------------------------------
Time: 0
States:
1 discrete state groups with
22 states
0 0 -1.57 0.1 0 -1.2 0 1.6 0 -0.05 0.05 0 0 0.308853 0.592595 0.23512 0.964971 0.945048 0.848401 0.472324 -0 0
Parameters:
63 numeric parameter groups with
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
1 parameters
0
1 parameters
1
10 parameters
nan nan nan nan nan nan nan nan nan nan
10 parameters
1 0 0 0 1 1 1 0 0 0
10 parameters
1 0 0 0 1 1 1 0 0 0
10 parameters
1 0 0 0 1 1 1 0 0 0
10 parameters
1 0 0 0 0.01042 0.00542 0.00542 0 0 0
10 parameters
1 0 0 0 0.01042 0.00542 0.00542 0 0 0
10 parameters
3.863 -0.013 0 0.07 0.00855001 0.00949561 0.00477682 0 0.00091 0
10 parameters
2.7025 0 -0.0347 0.113 0.0203006 0.0188005 0.00342426 0 0 0.0039211
10 parameters
2.7258 0 0.0668 0.0344 0.0118823 0.00712657 0.00670012 0 0 -0.00229792
10 parameters
3.175 0 0.0296 0.1265 0.0247524 0.0234983 0.00326986 0 0 -0.0037444
10 parameters
2.73 0 0.067 0.034 0.0118721 0.00709007 0.00672343 0 0 -0.002278
10 parameters
1.69 0.0001 0.021 0.076 0.0121342 0.0109239 0.00309781 -2.1e-06 -7.6e-06 -0.001596
10 parameters
1.8 0 0.0026 0.0005 0.00284034 0.00272247 0.0019512 0 0 -1.3e-06
10 parameters
0.4 0 0 0.0294 0.00186436 0.00186436 0.00125 0 0 0
10 parameters
0.988882 0 0 0 0.164825 0.164825 0.166667 0 0 0
10 parameters
0.05 0 0 0 3.2 3.2 3.2 0 0 0
10 parameters
0.05 0 0 0 3.2 3.2 3.2 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0.15 -0.284 0
12 parameters
1 0 0 0 1 0 0 0 1 -0.008 -0.1395 0
12 parameters
1 0 0 0 1 0 0 0 1 0.15 0.284 0
12 parameters
1 0 0 0 1 0 0 0 1 -0.008 0.1395 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0.1575
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
-1 -2.65359e-06 0 -9.74718e-12 3.67321e-06 1 -2.65359e-06 1 -3.67321e-06 0 0 0.183
12 parameters
1 -2.11758e-22 -4.23516e-22 2.11758e-22 1 -4.48416e-44 4.23516e-22 -4.48416e-44 1 0 5.55112e-17 -2.77556e-17
12 parameters
-1 -9.74718e-12 -2.65359e-06 -2.65359e-06 3.67321e-06 1 0 1 -3.67321e-06 5.29396e-23 0.184 -6.7587e-07
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 -3.67321e-06 1 0 -1 -3.67321e-06 0 0 0.2155
12 parameters
1 0 0 0 1 0 0 0 1 0 2.22045e-16 -1.11022e-16
12 parameters
-1 9.74718e-12 2.65359e-06 2.65359e-06 3.67321e-06 1 0 1 -3.67321e-06 0 0.184 -6.7587e-07
12 parameters
1 0 0 0 1 0 0 0 1 0 0 1.11022e-16
12 parameters
1 0 0 0 -3.67321e-06 1 0 -1 -3.67321e-06 0 0 0.2155
12 parameters
1 -2.11758e-22 -4.23516e-22 2.11758e-22 1 -4.48416e-44 4.23516e-22 -4.48416e-44 1 0 2.22045e-16 0
12 parameters
-1 -9.74718e-12 -2.65359e-06 -2.65359e-06 3.67321e-06 1 0 1 -3.67321e-06 -3.17637e-22 0.0805 -2.95693e-07
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0.049133 0
12 parameters
-1 0 -6.5359e-07 0 1 0 6.5359e-07 0 -1 -0.006 0.077133 0
12 parameters
1 0 0 0 1 0 0 0 1 -8.67362e-19 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0.006 0.077133 0
12 parameters
1 0 0 0 1 0 0 0 1 0 0 0
12 parameters
1 0 0 0 1 0 0 0 1 0 -0.049133 0
33 abstract parameters
::_::manipulation_station::scene_graph Context
-----------------------------------------------
Time: 0
Parameters:
1 abstract parameters
::_::manipulation_station::iiwa_controller Context (of a Diagram)
------------------------------------------------------------------
7 total continuous states
7 total numeric parameters in 1 groups
::_::manipulation_station::iiwa_controller::drake/systems/controllers/PidController@000000000344f9f0 Context
-------------------------------------------------------------------------------------------------------------
Time: 0
States:
7 continuous states
0 0 0 0 0 0 0
::_::manipulation_station::iiwa_controller::drake/systems/ConstantVectorSource@0000000003087830 Context
--------------------------------------------------------------------------------------------------------
Time: 0
Parameters:
1 numeric parameter groups with
7 parameters
0 0 0 0 0 0 0
::_::manipulation_station::desired_state_from_position Context (of a Diagram)
------------------------------------------------------------------------------
15 total discrete states in 3 groups
::_::manipulation_station::desired_state_from_position::drake/systems/DiscreteDerivative@0000000003308ac0 Context
------------------------------------------------------------------------------------------------------------------
Time: 0
States:
3 discrete state groups with
7 states
0 0 0 0 0 0 0
7 states
0 0 0 0 0 0 0
1 states
0
::_::manipulation_station::wsg_controller Context (of a Diagram)
-----------------------------------------------------------------
3 total discrete states in 3 groups
::_::manipulation_station::wsg_controller::drake/systems/StateInterpolatorWithDiscreteDerivative@00000000044811e0 Context (of a Diagram)
-----------------------------------------------------------------------------------------------------------------------------------------
3 total discrete states in 3 groups
::_::manipulation_station::wsg_controller::drake/systems/StateInterpolatorWithDiscreteDerivative@00000000044811e0::drake/systems/DiscreteDerivative@000000000346b830 Context
-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------
Time: 0
States:
3 discrete state groups with
1 states
0
1 states
0
1 states
0