class iiwa_sys():
def __init__(self,
builder,
dt=5e-4,
N=150,
params=None,
trj_decay=0.7,
x_w_cov=1e-5,
door_angle_ref=1.0,
visualize=False):
self.plant_derivs = MultibodyPlant(time_step=dt)
parser = Parser(self.plant_derivs)
self.derivs_iiwa, _, _ = self.add_models(self.plant_derivs,
parser,
params=params)
self.plant_derivs.Finalize()
self.plant_derivs_context = self.plant_derivs.CreateDefaultContext()
self.plant_derivs.get_actuation_input_port().FixValue(
self.plant_derivs_context, [0., 0., 0., 0., 0., 0., 0.])
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant_derivs.GetInputPort("applied_generalized_force").FixValue(
self.plant_derivs_context, null_force)
self.plant, scene_graph = AddMultibodyPlantSceneGraph(builder,
time_step=dt)
parser = Parser(self.plant, scene_graph)
self.iiwa, self.hinge, self.bushing = self.add_models(self.plant,
parser,
params=params)
self.plant.Finalize(
) # Finalize will assign ports for compatibility w/ the scene_graph; could be cause of the issue w/ first order taylor.
self.meshcat = ConnectMeshcatVisualizer(builder,
scene_graph,
zmq_url=zmq_url)
self.sym_derivs = False # If system should use symbolic derivatives; if false, autodiff
self.custom_sim = False # If rollouts should be gathered with sys.dyn() calls
nq = self.plant.num_positions()
nv = self.plant.num_velocities()
self.n_x = nq + nv
self.n_u = self.plant.num_actuators()
self.n_y = self.plant.get_state_output_port(self.iiwa).size()
self.N = N
self.dt = dt
self.decay = trj_decay
self.V = 1e-2 * np.ones(self.n_y)
self.W = np.concatenate((1e-7 * np.ones(nq), 1e-4 * np.ones(nv)))
self.W0 = np.concatenate((1e-9 * np.ones(nq), 1e-6 * np.ones(nv)))
self.x_w_cov = x_w_cov
self.door_angle_ref = door_angle_ref
self.q0 = np.array(
[-3.12, -0.17, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
#self.q0 = np.array([-3.12, -0.27, 0.52, -3.11, 1.22, -0.75, -1.56, 0.55])
self.x0 = np.concatenate((self.q0, np.zeros(nv)))
self.door_index = None
self.phi = {}
def ports_init(self, context):
self.plant_context = self.plant.GetMyMutableContextFromRoot(context)
self.plant.SetPositionsAndVelocities(self.plant_context, self.x0)
#door_angle = self.plant.GetPositionsFromArray(self.hinge, self.x0[:8])
self.door_index = self.plant.GetJointByName(
'right_door_hinge').position_start()
null_force = BasicVector([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
self.plant.GetInputPort("applied_generalized_force").FixValue(
self.plant_context, null_force)
self.plant.get_actuation_input_port().FixValue(
self.plant_context, [0., 0., 0., 0., 0., 0., 0.])
self.W0[self.door_index] = self.x_w_cov
def add_models(self, plant, parser, params=None):
iiwa = parser.AddModelFromFile(
"/home/hanikevi/drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision_no_grav.sdf"
)
plant.WeldFrames(plant.world_frame(),
plant.GetFrameByName("iiwa_link_0"))
#box = Box(10., 10., 10.)
#X_WBox = RigidTransform([0, 0, -5])
#mu = 0.6
#plant.RegisterCollisionGeometry(plant.world_body(), X_WBox, box, "ground", CoulombFriction(mu, mu))
#plant.RegisterVisualGeometry(plant.world_body(), X_WBox, box, "ground", [.9, .9, .9, 1.0])
#planar_joint_frame = plant.AddFrame(FixedOffsetFrame("planar_joint_frame", plant.world_frame(), RigidTransform(RotationMatrix.MakeXRotation(np.pi/2))))
X_WCylinder = RigidTransform([-0.75, 0, 0.5])
hinge = parser.AddModelFromFile(
"/home/hanikevi/drake/examples/manipulation_station/models/simple_hinge.sdf"
)
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base"),
X_WCylinder)
#cupboard_door_spring = plant.AddForceElement(RevoluteSpring_[float](plant.GetJointByName("right_door_hinge"), nominal_angle = -0.4, stiffness = 10))
if params is None:
bushing = LinearBushingRollPitchYaw_[float](
plant.GetFrameByName("iiwa_link_7"),
plant.GetFrameByName("handle"),
[50, 50, 50], # Torque stiffness
[2., 2., 2.], # Torque damping
[5e4, 5e4, 5e4], # Linear stiffness
[80, 80, 80], # Linear damping
)
else:
print('setting custom stiffnesses')
bushing = LinearBushingRollPitchYaw_[float](
plant.GetFrameByName("iiwa_link_7"),
plant.GetFrameByName("handle"),
[params['k4'], params['k5'], params['k6']], # Torque stiffness
[2, 2, 2], # Torque damping
[params['k1'], params['k2'], params['k3']], # Linear stiffness
[100, 100, 100], # Linear damping
)
bushing_element = plant.AddForceElement(bushing)
return iiwa, hinge, bushing
def cost_stage(self, x, u):
ctrl = 1e-5 * np.sum(u**2)
pos = 15.0 * (x[self.door_index] - self.door_angle_ref)**2
vel = 1e-5 * np.sum(x[8:]**2)
return pos + ctrl + vel
def cost_final(self, x):
return 50 * (1.0 * (x[self.door_index] - self.door_angle_ref)**2 +
np.sum(2.5e-4 * x[8:]**2))
def get_deriv(self, x, u):
self.plant_derivs.SetPositionsAndVelocities(self.plant_derivs_context,
x)
lin = FirstOrderTaylorApproximation(
self.plant_derivs, self.plant_derivs_context,
self.plant.get_actuation_input_port().get_index(),
self.plant.get_state_output_port(self.iiwa).get_index())
return lin.A(), lin.B(), lin.C()
def get_param_deriv(self, x, u):
# Using a closed-form solution; as currently DRAKE doesn't do support autodiff on parameters for LinearBusing.
# Note dC is 0 - stiffness does not affect measurements of q, v
W = self.plant.world_frame()
I = self.plant.GetFrameByName("iiwa_link_7")
H = self.plant.GetFrameByName("handle")
self.plant.SetPositionsAndVelocities(self.plant_context, x)
M = self.plant.CalcMassMatrixViaInverseDynamics(self.plant_context)
Jac_I = self.plant.CalcJacobianSpatialVelocity(
self.plant_context, JacobianWrtVariable.kQDot, I, [0, 0, 0], W, W)
Jac_H = self.plant.CalcJacobianSpatialVelocity(
self.plant_context, JacobianWrtVariable.kQDot, H, [0, 0, 0], W, W)
dA = np.zeros((self.n_x**2, 3))
dA_sq = np.zeros((self.n_x, self.n_x))
for param_ind in range(3):
JH = np.outer(Jac_H[param_ind, :], Jac_H[param_ind, :])
JI = np.outer(Jac_I[param_ind, :], Jac_I[param_ind, :])
dA_sq[8:, :8] = self.dt * np.linalg.inv(M).dot(JH + JI)
dA[:, param_ind] = deepcopy(dA_sq.ravel())
#print(np.sum(np.abs(dA), axis=0))
return dA
def reset(self):
x_traj_new = np.zeros((self.N + 1, self.n_x))
x_traj_new[0, :] = self.x0 + np.multiply(np.sqrt(self.W0),
np.random.randn(self.n_x))
u_traj_new = np.zeros((self.N, self.n_u))
#self.plant_context.SetDiscreteState(x_traj_new[0,:])
self.plant.SetPositionsAndVelocities(self.plant_context,
x_traj_new[0, :])
return x_traj_new, u_traj_new
def rollout(self):
self.u_trj = np.random.randn(self.N, self.n_u) * 0.001
self.x_trj, _ = self.reset()
for i in range(self.N):
self.x_trj[i + 1, :] = self.dyn(self.x_trj[i, :],
self.u_trj[i],
noise=True)
def dyn(self, x, u, phi=None, noise=False):
x_next = self.plant.AllocateDiscreteVariables()
self.plant.SetPositionsAndVelocities(self.plant_context, x)
self.plant.get_actuation_input_port(self.iiwa).FixValue(
self.plant_context, u)
self.plant.CalcDiscreteVariableUpdates(self.plant_context, x_next)
#print(x_next.get_mutable_vector().get_value())
x_new = x_next.get_mutable_vector().get_value()
if noise:
noise = np.multiply(np.sqrt(self.W), np.random.randn(self.n_x))
x_new += noise
return x_new
def obs(self, x, mode=None, noise=False, phi=None):
y = self.plant.get_state_output_port(self.iiwa).Eval(
self.plant_context)
#print(self.bushing.CalcBushingSpatialForceOnFrameA(self.plant_context).translational())
if noise:
y += np.multiply(np.sqrt(self.V), np.random.randn(self.n_y))
return y
def cost(self, x_trj=None, u_trj=None):
cost_trj = 0.0
if x_trj is None:
for i in range(self.N):
cost_trj += self.cost_stage(self.x_trj[i, :], self.u_trj[i, :])
cost_trj += self.cost_final(self.sys.plant, self.x_trj[-1, :])
else:
for i in range(self.N):
cost_trj += self.cost_stage(x_trj[i, :], u_trj[i, :])
cost_trj += self.cost_final(x_trj[-1, :])
return cost_trj
builder = DiagramBuilder()
# Adds both MultibodyPlant and SceneGraph and wires them together
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here
Parser(plant, scene_graph).AddModelFromFile(
FindResourceOrThrow(
"drake/manipulation/models/iiwa_description/sdf/iiwa14_no_collision.sdf"
))
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("iiwa_link_0"))
# Adds the MeshcatVisualizer and wires it to the Scene Graph
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, open_browser=True)
plant.Finalize()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
meshcat.load()
diagram.Publish(context)
plant_context = plant.GetMyMutableContextFromRoot(context)
plant.SetPositions(plant_context, [-1.57, 0.1, 0, -1.2, 0, 1.6, 0])
plant.get_actuation_input_port().FixValue(plant_context, np.zeros(7))
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
simulator.AdvanceTo(5)
