xfrc_applied #177

lucasmartins131 · 2022-03-10T00:42:54Z

lucasmartins131
Mar 10, 2022

Does anybody know if the funcion xfrc_applied of a generic body is applied in its CoM ?
And if its is applied in the body frame or inertial frame ?

I want to apply a generic force in body free in space (to simulate a uav). The forces are being computed by a controller, but when I apply then in the body of interest the results are not the expeted.

yuvaltassa · 2022-03-10T00:51:03Z

yuvaltassa
Mar 10, 2022
Maintainer

I am %90 sure they are applied at the inertial frame. To find out do the following. Make a free body with a child geom at some pos which is not (0, 0, 0). Set xfrc_applied for that body to be 0 0 m*g 0 0 0, i.e. make a gravity compensator. If it floats without spinning, the force is applied at the inertial frame :)

6 replies

yuvaltassa Mar 10, 2022
Maintainer

Okay one more followup, it is possible to apply a force/torque in a frame that is attached to the body, which is probably what you want for a flying thing. To do this, make an actuator with a site transmission, as explained here.

lucasmartins131 Mar 11, 2022
Author

Thanks very much for the help. I understood the site transmission, but I am not sure how to call it in my code. I have created 4 sites actuators in my xml. I am trying to use qfrc_actuator command to be equal to the controller calculated thrust need. But I am not sure if its the right way.

Thank's a lot in advance.

lucasmartins131 Mar 11, 2022
Author

Just to complement ... My xml looks like this. I only suprressed the four actuators and created a general actuator on CoM of my drone_box. Only to test. Then I made qfrc_actuator = [0 0 zforce 0 0 0]. But it does not move up

<worldbody>
    <body name = "drone_box">
        <joint name="body_drone" type="free" damping="0.01"/>
        <geom type="box" pos="0 0 0.075" size = "0.16 0.16 0.03" rgba ="0 0.5 0.5 1" quat="1 0 0 0" density = 50/>
            <site name="CG" pos="0 0 0.075" size="0.0001"/>
            <body name='rotor1' pos='0.16 0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                  <site name="r1" pos="0 0 0.075" size="0.0001"/>
		    </body>
		    <body name='rotor2' pos='0.16 -0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r2" pos="0 0 0.075" size="0.0001"/>
		    </body>
		    <body name='rotor3' pos='-0.16 -0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r3" pos="0 0 0.075" size="0.0001"/>
		    </body>
		    <body name='rotor4' pos='-0.16 0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r4" pos="0 0 0.075" size="0.0001"/>
		    </body>
    </body>
</worldbody>
<actuator>
<general site="CG" name="thrust_general" forcerange = "0 100"/>
</actuator>

erez-tom Mar 11, 2022
Collaborator

A general actuator can affect forces on the site in any one of the 6 DoFs, but you need a separate actuator for each direction. So if you want to actuate your box's CoM in all directions, you'd need to do something like:

<actuator>
  <general site="CG" name="fx" gear="1000 0 0 0 0 0"/>
  <general site="CG" name="fy" gear="0 1000 0 0 0 0"/>
  <general site="CG" name="fz" gear="0 0 1000 0 0 0"/>
  <general site="CG" name="tx" gear="0 0 0 100 0 0"/>
  <general site="CG" name="ty" gear="0 0 0 0 100 0"/>
  <general site="CG" name="tz" gear="0 0 0 0 0 100"/>
</actuator>

Note that the torque actuators and force actuators probably need a different scale of forces applied, as one is in Newton-meter and the other in Newton (or the equivalent units of your model).

lucasmartins131 Mar 11, 2022
Author

ok I updated the model, seams to be working in graphical interface (I changed manually the actuators). But in my general code I have an 6x1 actuation wrench calculated by my controller.
How do I apply it in my site actuator.
I tried to do qfrc_actuator = Wrench. But it did not moved
Sorry again if my questions are too basic.

erez-tom · 2022-03-11T13:51:57Z

erez-tom
Mar 11, 2022
Collaborator

Actuators are used by writing to ctrl and allowing the built-in machinery to compute qfrc_actuation.

2 replies

lucasmartins131 Mar 11, 2022
Author

Perfect. Now it is starting to make sense for me. But my results are not as expected.
What I made now is to make ctrl = Wrench = [Fx Fy Fz 0 0 0]. Again only to check if things are reasonable.
In thesis all of it applied in my site CoM created via xml.
But what is happening in fact is that my box is rotating in y axis.

Maybe I created the xml in the wrong way. I don't know .

    <geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" condim="1"/>
</worldbody>
<worldbody>
    <body name = "drone_box">
        <joint name="body_drone" type="free" damping="0.01"/>
        <geom type="box" pos="0 0 0.075" size = "0.16 0.16 0.03" rgba ="0 0.5 0.5 1" quat="1 0 0 0" density = 50/>
            <site name="CG" pos="0 0 0.075" size="0.0001"/>

        <body name='rotor1' pos='0.16 0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                  <site name="r1" pos="0 0 0.075" size="0.0001"/>
        </body>
        <body name='rotor2' pos='0.16 -0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r2" pos="0 0 0.075" size="0.0001"/>
        </body>
        <body name='rotor3' pos='-0.16 -0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r3" pos="0 0 0.075" size="0.0001"/>
        </body>
        <body name='rotor4' pos='-0.16 0.16 0.15'>
			<geom type="capsule" fromto="0 0 0 0 0 -0.05" size="0.005" contype="0" conaffinity="0" density = 0/>
                <site name="r4" pos="0 0 0.075" size="0.0001"/>
        </body>
    </body>
</worldbody>
<actuator>
    <general site="CG" name="fx" gear="1 0 0 0 0 0"/>
    <general site="CG" name="fy" gear="0 1 0 0 0 0"/>
    <general site="CG" name="fz" gear="0 0 1 0 0 0"/>
    <general site="CG" name="tx" gear="0 0 0 1 0 0"/>
    <general site="CG" name="ty" gear="0 0 0 0 1 0"/>
    <general site="CG" name="tz" gear="0 0 0 0 0 1"/>
</actuator>

yuvaltassa Mar 11, 2022
Maintainer

If you want to write pure joint-space forces use qfrc_applied, not qfrc_actuator. As Tom said, qfrc_actuator is overwritten by the engine. In order to understand what is wrong with your XML, inspect actuator_moment. This is the Jacobian matrix that maps from actuator_force to qfrc_applied.

Your XML looks fine on visual inspection, so I'm not sure what the issue is. Curious to hear what you find out.

xfrc_applied #177

Uh oh!

lucasmartins131 Mar 10, 2022

Replies: 2 comments · 8 replies

Uh oh!

yuvaltassa Mar 10, 2022 Maintainer

Uh oh!

yuvaltassa Mar 10, 2022 Maintainer

Uh oh!

lucasmartins131 Mar 11, 2022 Author

Uh oh!

lucasmartins131 Mar 11, 2022 Author

Uh oh!

erez-tom Mar 11, 2022 Collaborator

Uh oh!

lucasmartins131 Mar 11, 2022 Author

Uh oh!

erez-tom Mar 11, 2022 Collaborator

Uh oh!

lucasmartins131 Mar 11, 2022 Author

Uh oh!

Uh oh!

yuvaltassa Mar 11, 2022 Maintainer

lucasmartins131
Mar 10, 2022

Replies: 2 comments 8 replies

yuvaltassa
Mar 10, 2022
Maintainer

yuvaltassa Mar 10, 2022
Maintainer

lucasmartins131 Mar 11, 2022
Author

lucasmartins131 Mar 11, 2022
Author

erez-tom Mar 11, 2022
Collaborator

lucasmartins131 Mar 11, 2022
Author

erez-tom
Mar 11, 2022
Collaborator

lucasmartins131 Mar 11, 2022
Author

yuvaltassa Mar 11, 2022
Maintainer