def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
X_WE_desired = self.EvalAbstractInput(context, 0).get_value()
q_last = context.get_discrete_state_vector().get_value()
x = self.robot.tree().get_mutable_multibody_state_vector(
self.robot_context)
x[:robot.num_positions()] = q_last
result = DoDifferentialInverseKinematics(self.robot,
self.robot_context,
X_WE_desired, self.frame_E,
self.parameters)
if (result.status != result.status.kSolutionFound):
print("Differential IK could not find a solution.")
discrete_state.get_mutable_vector().SetFromVector(q_last)
else:
discrete_state.get_mutable_vector().\
SetFromVector(q_last + self.time_step*result.joint_velocities)
def _DoCalcDiscreteVariableUpdates(self, context, events, discrete_state):
rpy_xyz_desired = self.EvalVectorInput(context, 0).get_value()
X_WE_desired = RigidTransform(RollPitchYaw(rpy_xyz_desired[:3]),
rpy_xyz_desired[-3:]).GetAsIsometry3()
q_last = context.get_discrete_state_vector().get_value()
x = self.robot.GetMutablePositionsAndVelocities(self.robot_context)
x[:robot.num_positions()] = q_last
result = DoDifferentialInverseKinematics(self.robot,
self.robot_context,
X_WE_desired, self.frame_E,
self.parameters)
if (result.status != result.status.kSolutionFound):
print("Differential IK could not find a solution.")
discrete_state.get_mutable_vector().SetFromVector(q_last)
else:
discrete_state.get_mutable_vector().\
SetFromVector(q_last + self.time_step*result.joint_velocities)