def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
gripper_width_des = self.EvalVectorInput(
context, self.setpoint_input_port.get_index()).get_value()
q_full = x[:self.nq]
v_full = x[self.nq:]
tree = self.plant.tree()
q_hand = tree.get_positions_from_array(self.model_instance, q_full)
q_hand_des = np.array([-gripper_width_des[0], gripper_width_des[0]])
v_hand = tree.get_velocities_from_array(self.model_instance, v_full)
v_hand_des = np.zeros(2)
qerr_hand = q_hand_des - q_hand
verr_hand = v_hand_des - v_hand
Kp = 1000.
Kv = 100.
new_control_input[:] = np.clip(Kp * qerr_hand + Kv * verr_hand,
-self.max_force, self.max_force)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
q_des = self.EvalVectorInput(
context, self.setpoint_input_port.get_index()).get_value()
q_full = x[:self.nq]
v_full = x[self.nq:]
q = q_full[self.controlled_inds]
v = v_full[self.controlled_inds]
v_des = np.zeros(1)
qerr = q_des - q
verr = v_des - v
Kp = 1000.
Kv = 100.
new_control_input[:] = np.clip(Kp * qerr + Kv * verr, -self.max_force,
self.max_force)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
x_des = self.EvalVectorInput(
context, self.setpoint_input_port.get_index()).get_value()
q = x[:self.nq]
v = x[self.nq:]
q_des = x_des[:self.nq]
v_des = x_des[self.nq:]
qerr = (q_des[self.controlled_inds] - q[self.controlled_inds])
verr = (v_des[self.controlled_inds] - v[self.controlled_inds])
kinsol = self.rbt.doKinematics(q, v)
# Get the full LHS of the manipulator equations
# given the current config and desired accelerations
vd_des = np.zeros(self.rbt.get_num_positions())
vd_des[self.controlled_inds] = 1000. * qerr + 100 * verr
lhs = self.rbt.inverseDynamics(kinsol, external_wrenches={}, vd=vd_des)
new_u = self.B_inv.dot(lhs[self.controlled_inds])
new_control_input[:] = new_u
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_state = discrete_state.get_mutable_vector().get_mutable_value()
# Close gripper after plan has been executed
new_state[:] = self.gripper_setpoint_list[self.current_plan_idx]
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(
self, context, events, discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(context, 0).get_value()
old_u = discrete_state.get_mutable_vector().get_mutable_value()
new_u = self.ComputeControlInput(x, context.get_time())
new_control_input[:] = new_u
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_state = discrete_state. \
get_mutable_vector().get_mutable_value()
# Close gripper after plan has been executed
if context.get_time() > self.qtraj.end_time():
new_state[:] = 0.
else:
new_state[:] = 0.1
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
t = context.get_time()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
plan = self.EvalAbstractInput(
context, self.plan_input_port.get_index()).get_value()
q = x[:self.nq]
v = x[self.nq:]
tree = self.plant.tree()
q_kuka = tree.get_positions_from_array(self.model_instance, q)
v_kuka = tree.get_velocities_from_array(self.model_instance, v)
context = self.plant.CreateDefaultContext()
x_mutable = tree.get_mutable_multibody_state_vector(context)
x_mutable[:] = x
qDDt_desired = np.zeros(self.plant.num_velocities())
if plan.type == "JointSpacePlan":
q_kuka_ref = plan.traj.value(t - plan.t_start).flatten()
v_kuka_ref = plan.traj_d.value(t - plan.t_start).flatten()
qerr_kuka = (q_kuka_ref - q_kuka)
verr_kuka = (v_kuka_ref - v_kuka)
# Get the full LHS of the manipulator equations
# given the current config and desired accelerations
qDDt_desired[
self.controlled_inds] = 1000. * qerr_kuka + 100 * verr_kuka
lhs = tree.CalcInverseDynamics(context=context,
known_vdot=qDDt_desired,
external_forces=MultibodyForces(tree))
new_u = lhs[self.controlled_inds]
new_control_input[:] = new_u
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
state = discrete_state. \
get_mutable_vector().get_mutable_value()
if not self.initialized:
state[0] = self.STATE_CLEARING_KNIFE_AREA
self.initialized = True
t = context.get_time()
x_robot_full = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
kinsol = self.rbt.doKinematics(
x_robot_full[0:self.rbt.get_num_positions()])
# Legendary spaghetti starts here
if (isinstance(self.current_primitive, CutPrimitive)
and self.current_primitive.current_function_name != "done"):
# Currently cutting
pass
elif self.current_target_object is None:
# Search over objects, collating:
# 1) Whether they're in the knife zone already
# 2) What their max length along z axis is
cuttable_inds = []
cuttable_heights = []
under_blade_inds = []
under_blade_distances = []
end_effector_pos = self.rbt.transformPoints(
kinsol, np.zeros(3),
self.rbt.findFrame("iiwa_frame_ee").get_frame_index(), 0)
for body_i in self.world_builder.manipuland_body_indices:
current_object_pos = self.rbt.transformPoints(
kinsol,
self.rbt.get_body(body_i).get_center_of_mass(), body_i, 0)
if np.all(current_object_pos.T >= np.array([0.4, -0.6, 0.6])) and \
np.all(current_object_pos.T <= np.array([0.9, 0.6, 0.9])):
# This object is on the table! Measure its extent...
pts = self.rbt.get_body(body_i).get_visual_elements(
)[0].getGeometry().getPoints()
height = np.max(pts[2, :]) - np.min(pts[2, :])
print "Object with height %f" % height
if height >= 0.04:
cuttable_heights.append(height)
cuttable_inds.append(body_i)
# Whether or not it's cuttable, record if it's under
# the blade and its thinnest dimension is reasonable
thinnest_dimension = np.min(
np.max(pts, axis=1) - np.min(pts, axis=1))
print "And thinnest dimension %f" % thinnest_dimension
if thinnest_dimension >= 0.01 and \
np.all(current_object_pos.T >= np.array([0.4, -0.6, 0.6])) and \
np.all(current_object_pos.T <= np.array([0.7, 0.0, 0.9])):
under_blade_inds.append(body_i)
under_blade_distances.append(
np.linalg.norm(end_effector_pos -
current_object_pos))
n_clear_objects = len(under_blade_inds)
# What do we *want* to cut?
if len(cuttable_inds) == 0:
print "NOTHING LEFT TO CUT, ABORTING"
raise StopIteration
desired_cut_ind = cuttable_inds[np.argmax(cuttable_heights)]
# If it's under the blade, keep it there and move everything else
# out of the way.
cut_location = np.array([0.6, -0.2, 0.775])
clear_location = np.array(
[0.5 + np.random.random() * 0.2, 0.2, 0.825])
if (desired_cut_ind in under_blade_inds and n_clear_objects == 1) or \
(n_clear_objects == 0):
# Cut the object!
self.current_target_object_move_location = cut_location
self.current_target_object = desired_cut_ind
print "MOVING OBJECT %d FOR CUT" % self.current_target_object
self.do_cut_after_current_move = True
elif n_clear_objects > 0:
# Clear an object at random
self.current_target_object = random.choice(under_blade_inds)
while self.current_target_object == desired_cut_ind:
self.current_target_object = random.choice(
under_blade_inds)
print "CLEARING OBJECT %d" % self.current_target_object
self.current_target_object_move_location = clear_location
self.do_cut_after_current_move = False
self.current_primitive = MoveObjectPrimitive(
self.rbt,
self.q_nom,
self.current_target_object,
self.current_target_object_move_location,
target_yaw=np.pi / 2.)
else:
# Check that object location against current move goal
current_object_pos = self.rbt.transformPoints(
kinsol,
self.rbt.get_body(
self.current_target_object).get_center_of_mass(),
self.current_target_object, 0)
# Bail if we're really really lost
if np.linalg.norm(current_object_pos.T - self.current_target_object_move_location) > 2.0 \
or current_object_pos[2] < 0.5:
print "BAILING"
self.current_target_object = None
self.current_target_object_move_location = None
self.current_primitive = IdlePrimitive(self.rbt, self.q_nom)
self.do_cut_after_current_move = False
# Don't mode switch if we're currently moving super fast
if np.max(np.abs(x_robot_full[self.nq_full:][0:7])) >= 0.25:
# TODO switch this to
pass
elif self.do_cut_after_current_move:
# Check that the COM of the object is close to the blade line
if np.abs(current_object_pos[0] -
0.6) < 0.02 and current_object_pos[1] < -0.1:
print "EXECUTING CUT"
self.current_target_object = None
self.current_target_object_move_location = None
self.current_primitive = CutPrimitive(self.rbt, self.q_nom)
self.do_cut_after_current_move = False
else:
# Check that its' out of the blade area
if np.any(current_object_pos.T <= np.array([0.4, -0.6, 0.6])) or \
np.any(current_object_pos.T >= np.array([0.7, 0.0, 0.9])):
print "EXECUTING IDLE AFTER CLEAR"
self.current_target_object = None
self.current_target_object_move_location = None
self.current_primitive = IdlePrimitive(
self.rbt, self.q_nom)
context_info = TaskPrimitiveContextInfo(t, x_robot_full)
self.current_primitive.CalcSetpointsOutput(
context_info, self.kuka_setpoint.get_mutable_value(),
self.gripper_setpoint, self.knife_setpoint)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
# Call base method to ensure we do not get recursion.
# (This makes sure relevant event handlers get called.)
LeafSystem._DoCalcDiscreteVariableUpdates(self, context, events,
discrete_state)
new_control_input = discrete_state. \
get_mutable_vector().get_mutable_value()
x = self.EvalVectorInput(
context, self.robot_state_input_port.get_index()).get_value()
setpoint = self.EvalAbstractInput(
context, self.setpoint_input_port.get_index()).get_value()
q_full = x[:self.nq]
v_full = x[self.nq:]
q = x[self.controlled_inds]
v = x[self.nq:][self.controlled_inds]
kinsol = self.rbt.doKinematics(q_full, v_full)
M_full = self.rbt.massMatrix(kinsol)
C = self.rbt.dynamicsBiasTerm(kinsol, {}, None)[self.controlled_inds]
# Python slicing doesn't work in 2D, so do it row-by-row
M = np.zeros((self.nq_reduced, self.nq_reduced))
for k in range(self.nq_reduced):
M[:, k] = M_full[self.controlled_inds, k]
# Pick a qdd that results in minimum deviation from the desired
# end effector pose (according to the end effector frame's jacobian
# at the current state)
# v_next = v + control_period * qdd
# q_next = q + control_period * (v + v_next) / 2.
# ee_v = J*v
# ee_p = from forward kinematics
# ee_v_next = J*v_next
# ee_p_next = ee_p + control_period * (ee_v + ee_v_next) / 2.
# min u and qdd
# || q_next - q_des ||_Kq
# + || v_next - v_des ||_Kv
# + || qdd ||_Ka
# + || Kee_v - ee_v_next ||_Kee_pt
# + || Kee_pt - ee_p_next ||_Kee_v
# + the messily-implemented angular ones?
# s.t. M qdd + C = B u
# (no contact modeling for now)
prog = MathematicalProgram()
qdd = prog.NewContinuousVariables(self.nq_reduced, "qdd")
u = prog.NewContinuousVariables(self.nu, "u")
prog.AddQuadraticCost(qdd.T.dot(setpoint.Ka).dot(qdd))
v_next = v + self.control_period * qdd
q_next = q + self.control_period * (v + v_next) / 2.
if setpoint.v_des is not None:
v_err = setpoint.v_des - v_next
prog.AddQuadraticCost(v_err.T.dot(setpoint.Kv).dot(v_err))
if setpoint.q_des is not None:
q_err = setpoint.q_des - q_next
prog.AddQuadraticCost(q_err.T.dot(setpoint.Kq).dot(q_err))
if setpoint.ee_frame is not None and setpoint.ee_pt is not None:
# Convert x to autodiff for Jacobians computation
q_full_ad = np.empty(self.nq, dtype=AutoDiffXd)
for i in range(self.nq):
der = np.zeros(self.nq)
der[i] = 1
q_full_ad.flat[i] = AutoDiffXd(q_full.flat[i], der)
kinsol_ad = self.rbt.doKinematics(q_full_ad)
tf_ad = self.rbt.relativeTransform(kinsol_ad, 0, setpoint.ee_frame)
# Compute errors in EE pt position and velocity (in world frame)
ee_p_ad = tf_ad[0:3, 0:3].dot(setpoint.ee_pt) + tf_ad[0:3, 3]
ee_p = np.hstack([y.value() for y in ee_p_ad])
J_ee = np.vstack([y.derivatives()
for y in ee_p_ad]).reshape(3, self.nq)
J_ee = J_ee[:, self.controlled_inds]
ee_v = J_ee.dot(v)
ee_v_next = J_ee.dot(v_next)
ee_p_next = ee_p + self.control_period * (ee_v + ee_v_next) / 2.
if setpoint.ee_pt_des is not None:
ee_p_err = setpoint.ee_pt_des.reshape(
(3, 1)) - ee_p_next.reshape((3, 1))
prog.AddQuadraticCost(
(ee_p_err.T.dot(setpoint.Kee_pt).dot(ee_p_err))[0, 0])
if setpoint.ee_v_des is not None:
ee_v_err = setpoint.ee_v_des.reshape(
(3, 1)) - ee_v_next.reshape((3, 1))
prog.AddQuadraticCost(
(ee_v_err.T.dot(setpoint.Kee_v).dot(ee_v_err))[0, 0])
# Also compute errors in EE cardinal vector directions vs targets in world frame
for i, vec in enumerate(
(setpoint.ee_x_des, setpoint.ee_y_des, setpoint.ee_z_des)):
if vec is not None:
direction = np.zeros(3)
direction[i] = 1.
ee_dir_ad = tf_ad[0:3, 0:3].dot(direction)
ee_dir_p = np.hstack([y.value() for y in ee_dir_ad])
J_ee_dir = np.vstack([y.derivatives() for y in ee_dir_ad
]).reshape(3, self.nq)
J_ee_dir = J_ee_dir[:, self.controlled_inds]
ee_dir_v = J_ee_dir.dot(v)
ee_dir_v_next = J_ee_dir.dot(v_next)
ee_dir_p_next = ee_dir_p + self.control_period * (
ee_dir_v + ee_dir_v_next) / 2.
ee_dir_p_err = vec.reshape((3, 1)) - ee_dir_p_next.reshape(
(3, 1))
prog.AddQuadraticCost((ee_dir_p_err.T.dot(
setpoint.Kee_xyz).dot(ee_dir_p_err))[0, 0])
prog.AddQuadraticCost((ee_dir_v_next.T.dot(
setpoint.Kee_xyzd).dot(ee_dir_v_next)))
LHS = np.dot(M, qdd) + C
RHS = np.dot(self.B, u)
for i in range(self.nq_reduced):
prog.AddLinearConstraint(LHS[i] == RHS[i])
result = prog.Solve()
u = prog.GetSolution(u)
new_control_input[:] = u
