I am trying to add reaction wheels to a robotic spacecraft model in drake. For this, I am adding 3 revolute joints to the spacecraft base to simulate the torques applied on the base by the reaction wheels. Since URDF specification does not allow for spherical joints directly, I am using (pseudo) intermediate links to create a spherical joint. The relevant part of the urdf file is shown below:
<link name="Base"/>
<link name="Base_roll"/>
<!-- S/C Roll Joint -->
<joint name="Joint_base_roll" type="continuous">
<parent link="Base"/>
<child link="Base_roll"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<transmission name="Joint_base_roll_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="$Joint_base_roll_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="Joint_base_roll">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
</transmission>
<link name="Base_pitch"/>
<!-- S/C Pitch Joint -->
<joint name="Joint_base_pitch" type="continuous">
<parent link="Base_roll"/>
<child link="Base_pitch"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<transmission name="Joint_base_pitch_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="$Joint_base_pitch_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="Joint_base_pitch">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
</transmission>
<!-- S/C Yaw Joint -->
<joint name="Joint_base_yaw" type="continuous">
<parent link="Base_pitch"/>
<child link="Spacecraft"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<transmission name="Joint_base_yaw_trans">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="$Joint_base_yaw_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
<joint name="Joint_base_yaw">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
</transmission>
<!--Spacecraft-->
<link name="Spacecraft">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="2550"/>
<inertia ixx="6200" ixy="48.2" ixz="78.5" iyy="3540" iyz="-29.2" izz="7090"/>
</inertial>
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<box size="2.2 2.1 2" />
</geometry>
<material name="Grey"/>
</visual>
</link>
The full urdf for the spacecraft+robot arm with reaction wheels can be found here:https://pastebin.com/0G1GSeqk and without reaction wheels can be found here:https://pastebin.com/v9Ne8Nv8
I use this to run the following script to apply a torque to the base and simulate the rotation of the system accordingly:
builder = DiagramBuilder()
# Adds both MultibodyPlant and the SceneGraph, and wires them together.
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=1e-4)
# Note that we parse into both the plant and the scene_graph here.
Parser(plant, scene_graph).AddModelFromFile(SCpath)
# Remove gravity
plantGravityField = plant.gravity_field()
plantGravityField.set_gravity_vector([0,0,0])
plant.Finalize()
# Adds the MeshcatVisualizer and wires it to the SceneGraph.
meshcat = ConnectMeshcatVisualizer(builder, scene_graph, zmq_url=zmq_url, delete_prefix_on_load=True)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyMutableContextFromRoot(context)
jointAcutation = np.zeros((plant.num_actuators(),1))
jointAcutation[3] = 1
plant.get_actuation_input_port().FixValue(plant_context, jointAcutation)
simulator = Simulator(diagram, context)
meshcat.load()
simulator.AdvanceTo(30.0 if running_as_notebook else 0.1)
However, the Meshcat visualization only works without the reaction wheels part in the urdf. When I add the (pseudo) links and the joints the visualization is completely empty. Am I right to assume that this is due to the (pseudo) links in the urdf? If so, is the sdf format a better way to create spherical joints that they can also be visualized?
P.S I am trying to control the spacecraft attitude by applying torques on the base while moving the robot arm to simulate reaction wheels. I assume that adding a spherical joint would be the easiest way to add a "joint controller" to accomplish this. Maybe there is a better way to do this.
Update: This issue can be reproduced using the following notebook on Google Colab: https://colab.research.google.com/github/vyas-shubham/DrakeTests/blob/main/freeFloating/FreeFloatingTumbleTest.ipynb