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/Manipulation-ac8201a1-470a-4c77-afd0-2cc45bc229ff/project/6e32ce10-e406-4026-abfb-00be78c478cb/notebook/intro-7553c700044345fe8ce505eecfcf087f?duplicate=true&utm_medium=product-shared-content&utm_source=duplicate-cta&utm_campaign=duplicate&utm_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.
Yes, having the two sim plants the way I added them was causing the issue. Modifying the problem to have an internal sim plant for the controller (replacement for the robot model used by the controller) and another for the simulation (replacement for the real robot) fixed the issue.