def setUpClass(cls):
"""Setup and finalize the plant model, and create the optimization problem"""
cls.model = TimeSteppingMultibodyPlant(
file="systems/urdf/sliding_block.urdf")
cls.model.Finalize()
cls.opt = ContactImplicitDirectTranscription(
plant=cls.model,
context=cls.model.multibody.CreateDefaultContext(),
num_time_samples=101,
minimum_timestep=0.001,
maximum_timestep=0.1)
import matplotlib.pyplot as plt
from systems.terrain import FlatTerrain
from systems.timestepping import TimeSteppingMultibodyPlant
_file = "systems/urdf/sliding_block.urdf"
cc_erm_control = np.loadtxt('data/slidingblock/erm_w_cc/control.txt')
erm_control= np.loadtxt('data/slidingblock/erm/control.txt')
cc_erm_control = cc_erm_control.reshape([5, 101])
erm_control = erm_control.reshape([5, 101])
reference_control = np.loadtxt('data/slidingblock/warm_start/u.txt')
# u = u.reshape([1, 101])
h = 0.01
frictions = np.array([ 0.3, 0.43, 0.57, 0.7])
x0 = np.array([0, 0.5, 0, 0])
fig, axs = plt.subplots(3,1)
for i in range(len(frictions)):
plant = TimeSteppingMultibodyPlant(file= _file, terrain = FlatTerrain(friction = frictions[i]))
plant.Finalize()
control = reference_control
t, x, f = plant.simulate(h, x0, u = control.reshape(1,101), N = 101)
y_bar = np.zeros(t.shape) + 5
axs[0].plot(t, y_bar, 'k', linewidth =1)
axs[0].plot(t, x[0, :], linewidth =3, label = '$\mu$ = {f}'.format(f = frictions[i]))
axs[0].set_yticks([0, 2, 4, 6])
axs[0].set_ylim([0,6.1])
axs[0].set_xlim([0,1])
# Hide the right and top spines
axs[0].spines['right'].set_visible(False)
axs[0].spines['top'].set_visible(False)
axs[0].yaxis.set_ticks_position('left')
for i in range(len(frictions)):
from trajopt.contactimplicit import ContactImplicitDirectTranscription
from trajopt.robustContactImplicit import ChanceConstrainedContactImplicit
from systems.timestepping import TimeSteppingMultibodyPlant
from pydrake.solvers.snopt import SnoptSolver
from pydrake.math import RigidTransform, RollPitchYaw
import utilities as utils
from scipy.special import erfinv
import pickle
import os
from tempfile import TemporaryFile
from pydrake.all import PiecewisePolynomial
from systems.visualization import Visualizer
# Create the block model with the default flat terrain
_file = "systems/urdf/sliding_block.urdf"
plant = TimeSteppingMultibodyPlant(file=_file)
body_inds = plant.multibody.GetBodyIndices(plant.model_index)
base_frame = plant.multibody.get_body(body_inds[0]).body_frame()
plant.multibody.WeldFrames(plant.multibody.world_frame(), base_frame,
RigidTransform())
plant.Finalize()
# Get the default context
context = plant.multibody.CreateDefaultContext()
# set chance constraints parameters
beta, theta, sigma = 0.6, 0.6, 0.3
chance_params = np.array([beta, theta, sigma])
# set friction ERM parameters
friction_variance = sigma
friction_bias = 0.01
friction_multiplier = 1e6
# Imports import timeit import numpy as np import matplotlib.pyplot as plt # from trajopt.contactimplicit import ContactImplicitDirectTranscription from trajopt.robustContactImplicit import ChanceConstrainedContactImplicit from systems.timestepping import TimeSteppingMultibodyPlant from pydrake.solvers.snopt import SnoptSolver import utilities as utils from scipy.special import erfinv import pickle import os from tempfile import TemporaryFile from plotting_tool import plot_CC # Create the block model with the default flat terrain plant = TimeSteppingMultibodyPlant(file="systems/urdf/sliding_block.urdf") plant.Finalize() num_step = 101 step_size = 0.01 # Get the default context context = plant.multibody.CreateDefaultContext() # set chance constraints parameters beta, theta = 0.6, 0.6 sigmas = [0.01, 0.05, 0.1, 0.3, 1] # sigmas = [1] # sigmas = [1e-4, 1e-3, 1e-2, 1e-1, 1] times = [] # set friction ERM parameters friction_bias = 0.01 friction_multiplier = 1e6 # set uncertainty option
cc_erm_control = cc_erm_control.reshape([5, 101])
erm_control = erm_control.reshape([5, 101])
reference_control = np.loadtxt('data/slidingblock/warm_start/u.txt')
# u = u.reshape([1, 101])
h = 0.01
frictions = np.array([0.3, 0.4, 0.5, 0.6, 0.7])
x0 = np.array([0, 0.5, 0, 0])
reference = np.ones([101, 1]) * 5
reference_final_position = np.zeros(frictions.shape)
erm_final_position = np.zeros([5, len(frictions)])
cc_erm_final_position = np.zeros([5, len(frictions)])
final_position = np.zeros([11, len(frictions)])
# reference control
for i in range(len(frictions)):
plant = TimeSteppingMultibodyPlant(
file=_file, terrain=FlatTerrain(friction=frictions[i]))
plant.Finalize()
t, x_ref, f = plant.simulate(h,
x0,
u=reference_control.reshape(1, 101),
N=101)
final_position[0, i] = x_ref[0, 100]
for n in range(5):
t, x_erm, f = plant.simulate(h,
x0,
u=erm_control[n, :].reshape(1, 101),
N=101)
t, x_erm_cc, f = plant.simulate(h,
x0,
u=cc_erm_control[n, :].reshape(1, 101),
N=101)
def setUpClass(cls):
cls._model = TimeSteppingMultibodyPlant(
file="systems/urdf/fallingBox.urdf")
cls._model.Finalize()