Adding forces to body post-finalize?

Question

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.

score 2 · Accepted Answer · answered Jul 12 '19 at 15:55

@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.

score 1 · Answer 2 · answered Jul 11 '19 at 12:22

1

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.

answered Jul 11 '19 at 12:22

Sherm

643
4
6

gotcha! I'll look into doing something like this – Joaquin Giraldo-Laguna Jul 12 '19 at 13:11
working on this - it seems like calls to addinforce need to be made pre-finalize, correct? how is that possible when it requires a context to be added? I'm not sure when is the right time to call this – Joaquin Giraldo-Laguna Jul 12 '19 at 15:02
no need for a force element in this case. Please see my answer using an input port – Alejandro Jul 12 '19 at 15:56
That does seem easier! – Sherm Jul 14 '19 at 19:31

Adding forces to body post-finalize?

2 Answers2

Linked