CODEWITHSUNDEEP
c++simulationroboticsdrake
twSoulz
twSoulz

Reputation: 106

Implementing Aerodynamic Forces in Drake Simulation and Parsing Joint Angles

I am trying to calculate custom aerodynamic forces, applying them to a Multibodyplant (Robot parsed from an SDF) in Drake, and visualizing with Meshcat. Issues come up when computing velocity and aerodynamic calculations become inconsistent at some points (sometimes parsed velocity is drastically different than what is shown in simulation). This was partially fixed by decreasing timestep down to 0.00001.

How I set up force calculation and application is an external aerodynamics leafsystem that ports that connect to the multibodyplant:

    builder.Connect(aerodynamics_system.get_output_port(), plant.get_applied_spatial_force_input_port());
    builder.Connect(plant.get_body_poses_output_port(), aerodynamics_system.get_input_port(0));
    builder.Connect(plant.get_body_spatial_velocities_output_port(), aerodynamics_system.get_input_port(1));

I was wondering if this is a valid approach to implementing aerodynamics for sensitive small-scale flapping(50hz) robots. I was considering if there was a force application discrepancy within the system due to having separate leaf systems.

Another issue this build comes along with is trying to access joint angles as multibodyplant only supplies body poses and body spatial velocities, I had to use

right_wing_passive_joint_ = &plant_.GetJointByName<drake::multibody::RevoluteJoint>("joint_RW_J_Pitch");
std::court << right_wing_passive_joint_->get_angle(context) <<  std::endl;

This errors out in my aerodynamics leafsystem as the context argument requires the multibodyplant context instead of aerodynmaics leafsystem. Looking into this, I did a questionable method of creating a plant context within the CalcOutput function of my aerodynamics leafsystem and using that to evaluate the joint angle

    std::unique_ptr<drake::systems::Context<double>> plant_context = plant_.CreateDefaultContext();
    plant_context->SetTime(context.get_time());
    std::cout << "rw joint angle: " << right_wing_passive_joint_->get_angle(*plant_context) << std::endl;

This did not work and only resulted in a 0 value being printed, so I was wondering if I resort to calculating the angles from the body position and orientations.

I would appreciate advice on how to design this for efficient and optimized aerodynamics implementation and to be able to access the joint angles at every timestep of the simulation. Let me know if more code is needed.

Upvotes: 1

Views: 87

Answers (1)

Xuchen Han
Xuchen Han

Reputation: 391

The fact that you need tiny time steps for your simulation is probably due to loose coupling. Currently, the spatial force computation for your aerodynamics robot is lagged, i.e. the spatial force is computed as a function of the pose of the robot from the previous timestep (see https://github.com/RobotLocomotion/drake/issues/12786 for why that's the case). This can cause instability in highly dynamics scenes.

To your second question:

std::unique_ptr<drake::systems::Context<double>> plant_context = plant_.CreateDefaultContext();
    plant_context->SetTime(context.get_time());
std::cout << "rw joint angle: " << right_wing_passive_joint_->get_angle(*plant_context) << std::endl;

is the wrong approach. What it does is that it creates a default context of MultibodyPlant, sets its time to a certain value (without modifying anything else), and prints out the joint angle value, which by default is zero.

One way to feed the joint position to your controller is

right_wing_passive_joint_ = &plant_.GetJointByName<drake::multibody::RevoluteJoint>("joint_RW_J_Pitch");
// The starting index of the joint in the vector of generalized positions q for the plant.
const int position_start = right_wing_passive_joint_->position_start();
const BasicVector<double>& state = get_state_input_port().Eval(context);
const double joint_angle = state.value()(position_start);

This is assuming you have opened a multibody state input port for your controller and connected it to the state output port of your MultibodyPlant.

Upvotes: 0

Related Questions