Created
January 12, 2021 05:52
-
-
Save quanvuong/c11c96dd51e3a62195073cdf2beb4442 to your computer and use it in GitHub Desktop.
Drake weldframe un-intuitive behavior when <static> is included in sdf
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
<?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> |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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