CODEWITHSUNDEEP
drake
Hank
Hank

Reputation: 17

drake: trouble connecting plant output to visualizer input

The background is, I am controlling a quad tilt-wing with states x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙], and control input u=[ω1^2,ω2^2,ω3^2,ω4^2,θ1,θ2,θ3,θ4], here ωi are the speed of the propeller, and θi are the tilting angle of the wings. Basically I assume that I can control the tilting angles instantaneously, and I only care about the position and attitude of the body of the plane. Therefore, in my code, the num_states is 12, and num_inputs is 8.

For the purpose of visualization, I made a urdf file, where I set the tilting wings using revolute joints. And in drake, when I load the urdf using floating base, the rigidbodytree generates 20 states. This is not surprising, as I guess 12 of the states are x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙], and the extra 8 states come from the states of the revolute joints [θ1,θ2,θ3,θ4,θ1˙,θ2˙,θ3˙,θ4˙].

Just today when I was building the c++ code, this raises a problem if I use: builder.Connect(quad_tilt_wing->get_output_port(0), visualizer->get_input_port(0));

because 12 is not equal to 20.

So I am wondering how should I solve this? I am thinking about two ways:

(1) One is I modify my dynamics, so that my states are x=[X,Y,Z,ϕ,θ,ψ,X˙,Y˙,Z˙,ϕ˙,θ˙,ψ˙,θ1,θ2,θ3,θ4,θ1˙,θ2˙,θ3˙,θ4˙], and my controls are u=[ω21,ω22,ω23,ω24,θ1,θ2,θ3,θ4]. But in this way, I cannot think of a way of writing the dynamics in the form of x˙=f(x,u).

(2) The other way is, I am wondering if I can divide the input port of the visualizer into 2 ports, one is 12, and the other is 8, and then I feed the 12 with my dynamics x, the 8 with direct control using u. Is this doable?

Or do you have better way of addressing this?

Upvotes: 1

Views: 143

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

Take a look at http://drake.mit.edu/doxygen_cxx/group__primitive__systems.html

You should be able to DeMultiplex your input to get a signal that has just the theta values (that you want to patch into your visualization input). It's tempting to say you could Multiplex the two signals together into the signal you push to your visualizer input, but I think that you want to insert the inputs in the middle of the vector. So I suspect you'll want to do something like Multiplex then have a (matrix) Gain that permutes your vector elements into the right order.

Or, since you are writing the plant yourself, you can just make a second output port that contains the vector information that you need for visualization. That's the approach used in e.g. https://github.com/RobotLocomotion/drake/blob/master/examples/rimless_wheel/rimless_wheel.h#L31

Upvotes: 1

Related Questions