0

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

Shubham Vyas
  • 163
  • 10
  • 1
    The urdf files seem fine. I've verified that I can view them in meshcat using ```./bazel-bin/manipulation/util/geometry_inspector ~/Downloads/reaction.urdf --meshcat```. And yes -- it's fine to add these dummy joints if you need to add actuators to the "floating" joints. – Russ Tedrake Nov 01 '20 at 14:48
  • That's interesting. I don't see the model with meshcat.load() while using the reaction wheel version but see the model without reaction wheels in my python notebook. Furthermore, then I run the simulation and try to record it with meshcat.start_recording() I encounter the following error with meshcat: https://pastebin.com/jia2ynUp – Shubham Vyas Nov 01 '20 at 18:26
  • I suspect your system is going unstable during the simulation, and your states are going to nan? – Russ Tedrake Nov 02 '20 at 00:11
  • I see that as well. I am applying a 1Nm yaw torque to the base in the simulation and didn't expect nan states. But when I saw these I suspected they were because of the dummy links I created for the reaction wheels. Furthermore, I don't have a problem with simulation with reaction wheels if I do not use meshcat.start_recording(). Without meshcat recording, the simulation proceeds without any nan states. – Shubham Vyas Nov 02 '20 at 08:48
  • @RussTedrake I'm curious about the geometry_inspector tool. I'm using a binary installation of drake (as well the docker image, which I believe is essentially the same) and the geometry_inspector tool isn't built in this installation. Is it correct to assume I need to install drake from source (Github) to use the visualization tool? – Shubham Vyas Nov 09 '20 at 14:28
  • 1
    I think you are right that the geometry_inspector py file is not shipped with the drake binaries. But you can do something very similar in pydrake / jupyter. See this example: http://manipulation.csail.mit.edu/robot.html#example1 – Russ Tedrake Nov 11 '20 at 02:05
  • I do indeed already use the code from the manipulation course examples. It has been working well until now. Only when I change the urdf file string from the one without reaction wheels to the one with, the meshcat visualization does not show the robot. The simulation still runs successfully and the script only fails at the meshcat.start_recording() command. I'll share the notebook on google colab with the issue to recreate the situation better. – Shubham Vyas Nov 18 '20 at 11:30
  • @RussTedrake Here is the colab notebook to reproduce the issue I've been seeing by commenting/uncommenting the urdf file paths: [https://colab.research.google.com/github/vyas-shubham/DrakeTests/blob/main/freeFloating/FreeFloatingTumbleTest.ipynb](https://colab.research.google.com/github/vyas-shubham/DrakeTests/blob/main/freeFloating/FreeFloatingTumbleTest.ipynb) – Shubham Vyas Nov 18 '20 at 13:11
  • 1
    i just ran your code. If you add `print(context)` to the last line, you will see that you are getting nans from your simulation (while meshcat recording is still disabled). Meshcat could potentially handle the nans more gracefully, but it is not the source of your problem. – Russ Tedrake Nov 22 '20 at 00:38
  • 1
    Furthermore, it blows up immediately. I had to run it at a smaller timestep to even see non-nans at extremely small times. And if you set `time_step=0` in the multibodyplant constructor, you will see that even the error-controlled integration will complain. You definitely have a stiff system (the inertia of the main body is >> the inertia of the links), but I suspect there is something more wrong than that. I'll let you debug! – Russ Tedrake Nov 22 '20 at 00:50
  • Thank you very much for your help! I'll debug my model now :) – Shubham Vyas Nov 22 '20 at 12:47

0 Answers0