[Python] Reproducing the ManipulationStation for a custom robot

I am trying to setup a simulation with a robot + a block, with the task being the robot pushes the block defined area. For this purpose, Im trying to reproduce the example from Teleop Example (2D) notebook: https://deepnote.com/workspace/Mani..._content=6e32ce10-e406-4026-abfb-00be78c478cb

Ive seen other posts answered to have a look at the ManipulationStation class. However, when I try to do the same the robot free falls meaning the controller is not connected to the robot in the scene. Here is an example that does not work:


def AddXarm(plant, scene_graph=None):
if scene_graph is None:
parser = Parser(plant, scene_graph)
else:
parser = Parser(plant)
xarm = parser.AddModels(temp_urdf)[0]
return xarm

def add_block(plant, scene_graph=None):
parser = Parser(plant, scene_graph)
tblock = parser.AddModels(urdf_path)[0]
return tblock

def MakeHardwareStation():
robot_builder = RobotDiagramBuilder(time_step=time_step)
builder = robot_builder.builder()
sim_plant = robot_builder.plant()
scene_graph = robot_builder.scene_graph()
parser = Parser(sim_plant)
xarm = AddXarm(sim_plant, scene_graph)
sim_plant.Finalize()
xarm_positions = sim_plant.num_positions(xarm)
xarm_velocities = sim_plant.num_velocities(xarm)
controller_plant = sim_plant
control_num_positions = controller_plant.num_positions()
xarm_controller = builder.AddSystem(
InverseDynamicsController(
controller_plant,
kp=[100.0] * control_num_positions,
ki=[0.0] * control_num_positions,
kd=[20.0] * control_num_positions,
has_reference_acceleration=False,
)
)
builder.Connect(
sim_plant.get_state_output_port(),
xarm_controller.get_input_port_estimated_state(),
)
builder.Connect(
xarm_controller.get_output_port_control(),
sim_plant.get_actuation_input_port(),
)
desired_state_from_position = builder.AddSystem(
StateInterpolatorWithDiscreteDerivative(
xarm_positions, time_step, suppress_initial_transient=True
)
)
desired_state_from_position.set_name("desired_state_from_position")
builder.Connect(
desired_state_from_position.get_output_port(),
xarm_controller.get_input_port_desired_state(),
)

builder.ExportOutput(sim_plant.get_state_output_port(), "xarm_state_estimated")
builder.ExportInput(
desired_state_from_position.get_input_port(), "xarm_state_desired"
)

builder.ExportInput(
sim_plant.get_applied_generalized_force_input_port(),
"applied_generalized_force",
)
builder.ExportInput(
sim_plant.get_applied_spatial_force_input_port(),
"applied_spatial_force",
)
builder.ExportOutput(scene_graph.get_query_output_port(), "query_object")

diagram = robot_builder.Build()
diagram.set_name("station")
return diagram

class MultibodyPoseToConfig(LeafSystem):
def __init__(self, plant: MultibodyPlant, frame: Frame):
LeafSystem.__init__(self)
self.plant = plant
self.frame = frame
self.plant_context = plant.CreateDefaultContext()
self.DeclareAbstractInputPort(
"pose",
AbstractValue.Make(RigidTransform()),
)
self.DeclareVectorOutputPort(
"config",
6,
self._CalcOutput,
)

def _CalcOutput(self, context, output):
pose = self.get_input_port().Eval(context)
ik = InverseKinematics(self.plant, self.plant_context)
ik.AddPositionConstraint(
frameB=self.frame,
p_BQ=[0, 0, 0],
frameA=self.plant.world_frame(),
p_AQ_lower=pose.translation() - 1e-4,
p_AQ_upper=pose.translation() + 1e-4,
)
ik.AddOrientationConstraint(
frameAbar=self.frame,
R_AbarA=pose.rotation(),
frameBbar=self.plant.world_frame(),
R_BbarB=RotationMatrix(),
theta_bound=0.01,
)
prog = ik.prog()
result = Solve(prog)
q_desired = result.GetSolution(ik.q())
output.set_value(q_desired[:6])

def teleop_3d():
meshcat = StartMeshcat()
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=time_step)
block = add_block(plant, scene_graph)
xarm = AddXarm(plant, scene_graph)
add_ground_with_friction(plant)
plant.Finalize()
station = builder.AddSystem(MakeHardwareStation())
meshcat.DeleteAddedControls()
teleop = builder.AddSystem(
MeshcatPoseSliders(
meshcat,
initial_pose=RigidTransform(
RotationMatrix(RollPitchYaw(3.14, 0, 0)), np.array([0.2, 0.2, 0.2])
),
lower_limit=[0, -0.5, -np.pi, -0.6, -0.8, 0.0],
upper_limit=[2 * np.pi, np.pi, np.pi, 0.8, 0.3, 1.1],
)
)
ik_sys = builder.AddSystem(
MultibodyPoseToConfig(
plant, plant.GetBodyByName("push_gripper_base_link").body_frame()
)
)
builder.Connect(teleop.get_output_port(), ik_sys.get_input_port())
builder.Connect(
ik_sys.get_output_port(), station.GetInputPort("xarm_state_desired")
)

AddDefaultVisualization(builder, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
simulator.get_mutable_context()
simulator.set_target_realtime_rate(0)
simulator.AdvanceTo(np.inf)

teleop_3d()


I understand conceptually that the "sim robot" in the plant is driven by a model from the "control robot". But Im not sure whats going wrong in the actual implementation.

Continue reading...