def test_solve(self):
plant = MultibodyPlant(0)
M_AAo_A = SpatialInertia(1, np.zeros(3), UnitInertia(1, 1, 1))
body = plant.AddRigidBody("body", M_AAo_A)
plant.AddJoint(
PrismaticJoint("joint", plant.world_frame(), body.body_frame(),
[1, 0, 0]))
plant.Finalize()
path = PiecewisePolynomial.CubicWithContinuousSecondDerivatives(
[0., 1.], np.array([[0., 1.]]), [0.], [0.])
gridpoints = Toppra.CalcGridPoints(path, CalcGridPointsOptions())
toppra = Toppra(path=path, plant=plant, gridpoints=gridpoints)
toppra.AddJointAccelerationLimit([-1.], [1.])
trajectory = toppra.SolvePathParameterization()
self.assertIsInstance(trajectory, PiecewisePolynomial)
def add_arm_gripper(self, arm_name, arm_path, arm_base, X_arm,
gripper_path, arm_ee, gripper_base, X_gripper):
# Add arm
parser = Parser(self._mbp, self._sg)
arm_model_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = self._mbp.GetFrameByName(arm_base, arm_model_id)
self._mbp.WeldFrames(self._mbp.world_frame(), arm_base_frame, X_arm)
self._model_ids[arm_name] = arm_model_id
# Add gripper
gripper_name = arm_name+"_gripper"
arm_end_frame = self._mbp.GetFrameByName(arm_ee, arm_model_id)
self.add_floating_gripper(gripper_name, gripper_path, arm_end_frame, gripper_base, X_gripper)
# Add arm controller stack
ctrl_plant = MultibodyPlant(0)
parser = Parser(ctrl_plant)
ctrl_arm_id = parser.AddModelFromFile(arm_path, arm_name)
arm_base_frame = ctrl_plant.GetFrameByName(arm_base, ctrl_arm_id)
ctrl_plant.WeldFrames(ctrl_plant.world_frame(), arm_base_frame, X_arm)
gripper_equivalent = ctrl_plant.AddRigidBody(
gripper_name+"_equivalent", ctrl_arm_id,
self.calculate_ee_composite_inertia(gripper_path))
arm_end_frame = ctrl_plant.GetFrameByName(arm_ee, ctrl_arm_id)
ctrl_plant.WeldFrames(arm_end_frame, gripper_equivalent.body_frame(),
X_gripper)
ctrl_plant.Finalize()
self._control_mbp[arm_name] = ctrl_plant
arm_num_positions = ctrl_plant.num_positions(ctrl_arm_id)
kp = 4000*np.ones(arm_num_positions)
ki = 0 * np.ones(arm_num_positions)
kd = 5*np.sqrt(kp)
arm_controller = self._builder.AddSystem(InverseDynamicsController(
ctrl_plant, kp, ki, kd, False))
adder = self._builder.AddSystem(Adder(2, arm_num_positions))
state_from_position = self._builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
arm_num_positions, self._mbp.time_step(), True))
# Add command pass through and state splitter
arm_command = self._builder.AddSystem(PassThrough(arm_num_positions))
state_split = self._builder.AddSystem(Demultiplexer(
2*arm_num_positions, arm_num_positions))
def finalize_func():
builder = self._builder
# Export positions commanded
command_input_name = arm_name + "_position"
command_output_name = arm_name + "_position_commanded"
self._port_names.extend([command_input_name, command_output_name])
builder.ExportInput(arm_command.get_input_port(0), command_input_name)
builder.ExportOutput(arm_command.get_output_port(0), command_output_name)
# Export arm state ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
state_split.get_input_port(0))
arm_q_name = arm_name + "_position_measured"
arm_v_name = arm_name + "_velocity_estimated"
arm_state_name = arm_name + "_state_measured"
self._port_names.extend([arm_q_name, arm_v_name, arm_state_name])
builder.ExportOutput(state_split.get_output_port(0), arm_q_name)
builder.ExportOutput(state_split.get_output_port(1), arm_v_name)
builder.ExportOutput(self._mbp.get_state_output_port(arm_model_id),
arm_state_name)
# Export controller stack ports
builder.Connect(self._mbp.get_state_output_port(arm_model_id),
arm_controller.get_input_port_estimated_state())
builder.Connect(arm_controller.get_output_port_control(),
adder.get_input_port(0))
builder.Connect(adder.get_output_port(0),
self._mbp.get_actuation_input_port(arm_model_id))
builder.Connect(state_from_position.get_output_port(0),
arm_controller.get_input_port_desired_state())
builder.Connect(arm_command.get_output_port(0),
state_from_position.get_input_port(0))
torque_input_name = arm_name + "_feedforward_torque"
torque_output_cmd_name = arm_name + "_torque_commanded"
torque_output_est_name = arm_name + "_torque_measured"
self._port_names.extend([torque_input_name, torque_output_cmd_name,
torque_output_est_name])
builder.ExportInput(adder.get_input_port(1), torque_input_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_cmd_name)
builder.ExportOutput(adder.get_output_port(0), torque_output_est_name)
external_torque_name = arm_name + "_torque_external"
self._port_names.append(external_torque_name)
builder.ExportOutput(
self._mbp.get_generalized_contact_forces_output_port(arm_model_id),
external_torque_name)
self._finalize_functions.append(finalize_func)