CODEWITHSUNDEEP
drake
Joaquin Giraldo-Laguna
Joaquin Giraldo-Laguna

Reputation: 65

Adding forces to body post-finalize?

I'm setting up a simple simulation where I want a body (a box) to fall in a gravity field for half a second and then rise up due to a force I apply on it which overcomes gravity.

I've tried implementing this force using three methods, using AddInForce, AddInForceInWorld, and AddForceElement. Since this is called while the simulation is running, AddForceElement threw an error as expected. With the other two, though, no errors or warnings were thrown, yet the box clearly does not have a new force acting on it.

I've checked for a new force element with num_force_elements() too but none are added.

Here is my loop for timing this event:

    while( current_time < FLAGS_duration && !terminate){
        if (current_time > 0.5 && !forced) {

            std::cout << "\nAdding Force of type " << FLAGS_box_f << " at 0.5 seconds...\n";

            // Add Rising Force to box
            auto forces = drake::multibody::MultibodyForces<double>(plant);
            drake::multibody::SpatialForce<double> forceup(Vector3d::Zero(), Vector3d(0, 0, 100));

            if (FLAGS_box_f == "spring") {
                plant.AddForceElement<LinearSpringDamper>(
                    plant.GetBodyByName("Box"), Vector3d::Zero(),
                    plant.world_body(), Vector3d(0, 0, 1),
                    0., 10., 1.);

            } else if (FLAGS_box_f == "world") {
                plant.GetBodyByName("Box").AddInForceInWorld(
                    plant_context,
                    forceup,
                    &forces);

            } else {
                plant.GetBodyByName("Box").AddInForce(
                    plant_context,
                    Vector3d::Zero(),
                    forceup,
                    plant.GetBodyByName("Box").body_frame(),
                    &forces);
            }

            plant.CalcForceElementsContribution(plant_context, &forces);
            std::cout << "Plant: " << plant.num_force_elements() << " force_elements\n";
            forced = true;
        }

        simulator.StepTo(current_time + time_delta);
        current_time = simulator_context.get_time();
    }

I'm not sure if I'm going through the motions right, and also still don't understand very well the MultibodyForces object I am storing the forces in the calls to.

Upvotes: 3

Views: 248

Answers (2)

Alejandro
Alejandro

Reputation: 1099

@Joaquin Giraldo,

The way to add external forces to your model is with an input force. Everything in Drake are input/output ports. In this case, you must use MultibodyPlant::get_applied_spatial_force_input_port(). See the file multibody/plant/test/externally_applied_spatial_force_test.cc for an example on how to do this.

Upvotes: 2

Sherm
Sherm

Reputation: 678

Quick note: what you need is a force element that is always present, but varies its force output based on time. None of the built-in ones behave that way, so you will have to write one.

Upvotes: 1

Related Questions