How to implement a Position-Based servo on URDF model and decrease the dimension of state vector

[xuhao1]

I am working on using drake for controlling airplane with vectored-thrust electrical jets. After reading https://github.com/RobotLocomotion/drake/issues/63 I understand that for keeping formula like A*Xdot = B X + C u, drake only provide torque based servo. But when I want to make a simple LQR controller for hovering.and I use following vectored-thrust

<joint 
        name="nozzle_left"
        type="revolute">
        <origin xyz="0.8 -0.12 0" rpy="0 0 0"/>
        <parent link="fuselage" />
        <child link="thrust_nozzle_left" />
        <axis xyz="0 1 0"/>
        <limit lower="-3.14/6" upper="3.14/6"/>
    </joint>

    <joint 
        name="nozzle_right"
        type="revolute">
        <origin xyz="0.8 0.12 0" rpy="0 0 0"/>
        <parent link="fuselage" />
        <child link="thrust_nozzle_right" />
        <axis xyz="0 1 0"/>
        <limit lower="-3.14/6" upper="3.14/6"/>
    </joint>

    <force_element name="thrust_left">
        <thrust lower_limit="0" upper_limit="36" scale_factor_thrust="1.0">
            <parent link="thrust_nozzle_left" />
            <origin xyz="0 0 0"/>
            <axis xyz="1 0 0"/>
        </thrust>
    </force_element>

    <force_element name="thrust_right">
        <thrust lower_limit="0" upper_limit="36" scale_factor_thrust="1.0">
            <parent link="thrust_nozzle_right" />
            <origin xyz="0 0 0"/>
            <axis xyz="1 0 0"/>
        </thrust>
    </force_element>
    <transmission name="rotate_left_thrust" type="SimpleTransmission" >
        <joint name="nozzle_left" />
        <actuator name="nozzle_left_servo"/>
        <mechanicalReduction>1</mechanicalReduction>
    </transmission>
    <transmission name="rotate_right_thrust" type="SimpleTransmission" >
        <joint name="nozzle_right" />
        <actuator name="nozzle_right_servo"/>
        <mechanicalReduction>1</mechanicalReduction>
    </transmission>

the angular velocity of nozzle_left is redundant because when in the application I can only control the angle of servo as input. I want to know is there some simple method to make a inner-loop(like pd controller) and thus decrease the state of angular velocity of the nozzle joint of the robot(So that I can direct make a LQR controller based on the RigidBodyManipulator)?

More generally, sometimes direct use RigidBodyManipulator import from urdf seems difficult, and we need some advance operator on the Manipulator (like make the controller of nozzle to be a inner loop).Is there any document or tourtial or example code for this?

Thanks a lot.

[RussTedrake]

see also #107, which is probably the most relevant.

the short answer is that, although we have sketched a few implementations, we did not push any to master in the matlab code. this is because having a position-controlled actuator violates the contract of being a rigid body manipulator (dynamics have the form Hvdot + C = Bu, ...).

The good news is that we're very actively redoing the rigid body dynamics engine in C++ directly, and have made a different design choice from the beginnig to support this (a flat class hiearchy, which can be interrogated at runtime to see if the dynamics support the structure of e.g. the rigid body mechanics). This removes the unwanted constraint. It's coming online very fast; I would expect we will have it in the next few weeks. Until then, you can have systems wrapped with PD control which have the current input and output dimension, but the internal state dimension of your system will be the full state space (positions and velocities).

[xuhao1]

Alright, I have done for a ugly way and expect for your new system :+1:

[xuhao1]

And thanks for your suggestion!

[RussTedrake]

great. will close this one.

[aidgab]

The good news is that we're very actively redoing the rigid body dynamics engine in C++ directly, and have made a different design choice from the beginnig to support this

How it's going on with redoing the things? I'm particularly interested in implementing position servo control for Nao.