Skip to content

Instantly share code, notes, and snippets.

@quanvuong
Created January 12, 2021 05:52
Show Gist options
  • Save quanvuong/c11c96dd51e3a62195073cdf2beb4442 to your computer and use it in GitHub Desktop.
Save quanvuong/c11c96dd51e3a62195073cdf2beb4442 to your computer and use it in GitHub Desktop.
Drake weldframe un-intuitive behavior when <static> is included in sdf
<?xml version='1.0'?>
<sdf version="1.7">
<model name="my_model">
<pose>0 0 0.5 0 0 0</pose>
<static>true</static>
<link name="box_link">
<inertial>
<mass>1.0</mass>
<inertia> <!-- inertias are tricky to compute -->
<!-- http://gazebosim.org/tutorials?tut=inertia&cat=build_robot -->
<ixx>0.083</ixx> <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) -->
<ixy>0.0</ixy> <!-- for a box: ixy = 0 -->
<ixz>0.0</ixz> <!-- for a box: ixz = 0 -->
<iyy>0.083</iyy> <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) -->
<iyz>0.0</iyz> <!-- for a box: iyz = 0 -->
<izz>0.083</izz> <!-- for a box: izz = 0.083 * mass * (x*x + y*y) -->
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</collision>
<visual name="visual">
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
</link>
</model>
</sdf>
from meshcat.servers.zmqserver import start_zmq_server_as_subprocess
from pydrake.all import (
DiagramBuilder,
AddMultibodyPlantSceneGraph,
FindResourceOrThrow,
ConnectMeshcatVisualizer,
Parser,
RigidTransform,
RollPitchYaw
)
proc, zmq_url, web_url = start_zmq_server_as_subprocess()
def visualize_gripper_frames(X_G):
builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(
builder, time_step=0.0
)
parser = Parser(plant, scene_graph)
for key, pose in X_G.items():
g = parser.AddModelFromFile(
'./box.sdf',
f'location_{key}'
)
plant.WeldFrames(
plant.world_frame(),
plant.GetFrameByName('box_link', g),
pose
)
plant.Finalize()
meshcat = ConnectMeshcatVisualizer(
builder, scene_graph, zmq_url=zmq_url
)
meshcat.load()
diagram = builder.Build()
context = diagram.CreateDefaultContext()
diagram.Publish(context)
transforms = {}
for i in range(5):
transforms[i] = RigidTransform(
RollPitchYaw([0., 0., 0.]),
[0., 0., 0.]
)
print('Number of frame to draw', len(transforms))
visualize_gripper_frames(transforms)
while True:
continue
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment