Simple model not working, please help

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
allan
Posts: 8
Joined: Wed Jul 28, 2021 1:30 am

Simple model not working, please help

Post by allan »

I have created an extremely simple model of a rotor rotating around the z-axis. The base link is a heavy rectangular box, the rotor is a single link representing two opposing rotor blades, and there is a continuous joint with a vertical axis between the links. When I use velocity control of the joint motor, calculating the acceleration and velocity myself based on the applied torque, it works nicely. When I use torque control for the motor, the results seem ridiculous. With a moment of inertia of 0.1512 and an applied torque of 0.0001 Nm, starting from rest, the initial angular acceleration is 150 radians/s^2. This decreases over the first 0.22 seconds to 113 rad/s^2. Then I reverse the torque, resulting in an initial acceleration of -187 rad/s^2, gradually decreasing to -112 rad/s^2 over the next 0.4 seconds. Obviously I must be doing something wrong, but what?

Here is the code:

Code: Select all

# disable default velocity control of rotor hub joint
p.setJointMotorControl2(gyro, rotor_joint_index,
                     controlMode=p.VELOCITY_CONTROL, force=0)
# set maximum velocity of joint                              
p.changeDynamics(gyro, rotor_joint_index, maxJointVelocity=1000)

#initialize simulation state
istep = 0
dt = 1.0/250.0    # time step = 4 ms
p.setTimeStep(dt) 
torque = 0.0001   
last_v = 0
while True:
    info = p.getJointState(gyro, rotor_joint_index)
    v = info[1] # angular velocity from bullet
    dv = v - last_v
    a = dv/dt 
    last_v = v
    rpm = v*60/(2*math.pi)    # rpm from angular velocity
    print("time=%.3f(s)," % (istep*dt), "applied torque=%8.5f(Nm)," % (torque), \
            "v=%7.3f(rad/s)" % (v), "a=%9.3f(rad/s^2)" % (a))
    if v > 30:
      torque = -0.0001
    elif v < -30:
      torque = 0.0001
    p.setJointMotorControl2(gyro, rotor_joint_index,
                                controlMode=p.TORQUE_CONTROL, 
                                force=torque)				
    p.stepSimulation()
    istep = istep + 1
    sleep(1/10)
Here is my URDF model:

Code: Select all

    <link name="base_link">
        <visual> 
            <geometry> 
                <box size="0.5 0.3 0.1"/>
            </geometry>
            <origin xyz = "0 0 0" rpy="0 0 0"/> 
            <material name="blue"/>
        </visual>
         <collision> 
            <geometry> 
                <box size="0.5 0.3 0.1"/>
            </geometry>
            <origin xyz = "0 0 0" rpy="0 0 0"/>
       </collision> 
       <inertial> 
            <origin xyz = "0 0 0" rpy="0 0 0"/>
            <mass value="100"/>
            <inertia ixx="100" ixy="0" ixz="0" iyy="100" iyz="0.0" izz="100"/>
        </inertial> 
    </link> 
    
    <link name="rotor">
        <visual> 
            <geometry> 
                <box size="2 0.05 0.01"/>
            </geometry>
            <origin xyz="0 0 0.0" rpy="0 0 0"/>  <!-- offset of child frame from the joint -->
            <material name="black"/>
        </visual>
        <inertial> 
            <origin xyz="0 0 0.0" rpy="0 0 0"/>
            <mass value="1"/>
            <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.1512"/>
        </inertial> 
    </link> 
    <joint name="base_to_hub" type="continuous">
        <parent link="base_link"/>
        <child link="rotor"/> 
        <axis xyz="0 0 1"/>     <!-- vector of the axis of rotation -->
        <origin xyz="0 0 0.2" rpy="0 0 0"/>  <!-- transform from parent frame to child frame -->
    </joint>
Any help would be appreciated. Thx.
allan
Posts: 8
Joined: Wed Jul 28, 2021 1:30 am

Re: Simple model not working, please help

Post by allan »

Problem partly solved. I had not used the URDF_USE_INERTIA_FROM_FILE flag in loadURDF, so PyBullet was defaulting to an inertia tensor of zero. When I included the flag, I got an error message "Bad inertia tensor properties, setting inertia to zero for link", apparently because in my URDF model Izz was larger than the sum of Ixx and Iyy. Fixed that and now the rotor starts accelerating correctly, but as the velocity increases the acceleration gradually decreases and eventually reaches zero, although the applied torque remains the same. Joint damping and friction are both reported as zero.
allan
Posts: 8
Joined: Wed Jul 28, 2021 1:30 am

Re: Simple model not working, please help

Post by allan »

Since torque control of the motor is not working, as a workaround I can calculate the expected acceleration from the torque, integrate for the velocity, and use velocity control of the rotor. I am using this in another application (not the one I posted above), where I calculate the torque from aerodynamic forces, including lift and drag. This works, and the rotor reaches a terminal velocity of about 290 RPM where the torque drops to zero, which is what I would expect. However, in this condition getJointState() reports an applied torque of about 1 N. Why is this needed for a constant velocity?
Post Reply