dynamic simulation of actuated pendulum reveals unclear behaviors

Typically: "How do I... ", "How can I... " questions
Post Reply
marcofe
Posts: 16
Joined: 27 Apr 2015, 14:25

dynamic simulation of actuated pendulum reveals unclear behaviors

Post by marcofe »

Hi all,

I'm practicing with dynamic simulation in CoppeliaSim and trying to figure out how things work under the hood. I've read the related pages of the documentation, but I couldn't find there answers to some questions arisen from some tests I did.

In detail, I created a very simple actuated pendulum as a pure, dynamic cuboid that is child of a dynamically enabled joint, with control loop enabled. Through the following embedded script, I commanded the joint at the torque level with a sinusoidal torque input and integrated manually the corresponding joint velocity and position from that command, so as to compare such predicted values with the measurements provided by the API functions sim.getJointPosition, sim.getJointVelocity and sim.getJointForce.

Code: Select all

function sysCall_init()
    -- do some initialization here
    
    joint = sim.getObjectHandle('Joint')
    link = sim.getObjectHandle('Link')
    graph = sim.getObjectHandle('jointGraph')
    q_graph = sim.getObjectHandle('qGraph')
    qd_graph = sim.getObjectHandle('qdotGraph')
    
    taumeas_str = sim.addGraphStream(graph,'tau_meas','Nm',0,{0,0,1})
    taucmd_str = sim.addGraphStream(graph,'tau_cmd','Nm',0,{1,0,0})
    qmeas_str = sim.addGraphStream(q_graph,'q1','Nm',0,{1,0,0})
    qmod_str = sim.addGraphStream(q_graph,'q1_mod','Nm',0,{0,0,1})
    qdmeas_str = sim.addGraphStream(qd_graph,'qd1','Nm',0,{1,0,0})
    qdmod_str = sim.addGraphStream(qd_graph,'qd1_mod','Nm',0,{0,0,1})

    Ts = sim.getSimulationTimeStep()
    
    t = 0.0
    taumeas = 0.0
    q = 0.0
    qd = 0.0
    q_mod = 0.0
    qd_mod = 0.0
    qdd_mod = 0.0
    g0 = 9.81

    qdprev_mod = qd_mod
    qprev_mod = q_mod
    
    inertiaMatrix, transformationMatrix=sim.getShapeInertia(link)
    mass = sim.getShapeMass(link)
    Iz = inertiaMatrix[9]
    r1, r2, size = sim.getShapeGeomInfo(link)
    d = size[3]/2
    print(d)
end

function sysCall_actuation()
    -- put your actuation code here
    
    
    t = sim.getSimulationTime()
    T = 20.
    u = -0.05 * math.sin(2*math.pi*t/T)

    qdd_mod = (u - mass * g0 * d * math.sin(q_mod))/Iz
    qd_mod = qdprev_mod + qdd_mod * Ts

    q_mod = qprev_mod + qd_mod * Ts + 0.5 * qdd_mod * Ts^2
    
    qdprev_mod = qd_mod
    qprev_mod = q_mod

    sim.setJointMaxForce(joint,math.abs(u))
    sim.setJointTargetPosition(joint,1e10*sign(u))
    
    sim.setGraphStreamValue(graph,taucmd_str,-u)
    sim.setGraphStreamValue(graph,taumeas_str,taumeas)
    sim.setGraphStreamValue(q_graph,qmeas_str,q)
    sim.setGraphStreamValue(q_graph,qmod_str,q_mod)
    sim.setGraphStreamValue(qd_graph,qdmeas_str,qd)
    sim.setGraphStreamValue(qd_graph,qdmod_str,qd_mod)
    
end

function sysCall_sensing()
    -- put your sensing code here
    --t = sim.getSimulationTime()
    taumeas = sim.getJointForce(joint)
    q = sim.getJointPosition(joint)
    qd = sim.getJointVelocity(joint)
end

function sysCall_cleanup()
    -- do some clean-up here
end

function sign(x)
    if(x > 0) then
        return 1
    elseif(x < 0) then
        return -1
    else
        return 0
    end
end

However, when running the simulation I observe that, while commanded and measured torques correctly match, the integrated joint position and velocity and their corresponding measurements do not, and such mismatch does not seem depending on an integration error. In particular, measured velocity and position exhibit, apart from a sinusoidal profile of the same frequency as the measured torque, also an additional sinusoidal component with different frequency, that I wouldn't know what could be caused by. This is also qualitatively observed in the simulation, that shows the pendulum moving with a double-frequency oscillation.

I supposed that this could some phenomenon depending on the dynamic integration and simulation time steps, even if couldn't find an exact connection. Still, it is indeed an undesired behavior that I would like to remove, so any suggestions about the possible reasons for this is strongly and well accepted.

Thanks in advance

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: dynamic simulation of actuated pendulum reveals unclear behaviors

Post by coppelia »

Hello,

I think the best in your situation is to write your own controller, via a joint callback function, the main reason is that it will allow you to write the exact position controller that you need (by default that controller is a simple P controller), and additionally to handle control values in each dynamic simulation step: remember that by default the physics engine will run 10 sub-step for one simulation step. Currently you are feeding the controller 1 new value for each simulation step (script actuation phase), but the physics engine will then run the controller 10 times for that value, similar to what is illustrated here:

Code: Select all

    Simulation step start (i.e. t=t+dt)
             .
             .
             .
       Actuation phase   --> sysCall_actuation
             .
             .
             .
Physics engine run for t+1*dt/10   --> sysCall_jointCallback
Physics engine run for t+2*dt/10   --> sysCall_jointCallback
Physics engine run for t+3*dt/10   --> sysCall_jointCallback
Physics engine run for t+4*dt/10   --> sysCall_jointCallback
Physics engine run for t+5*dt/10   --> sysCall_jointCallback
Physics engine run for t+6*dt/10   --> sysCall_jointCallback
Physics engine run for t+7*dt/10   --> sysCall_jointCallback
Physics engine run for t+8*dt/10   --> sysCall_jointCallback
Physics engine run for t+9*dt/10   --> sysCall_jointCallback
Physics engine run for t+10*dt/10  --> sysCall_jointCallback
             .
             .
             .
        Sensing phase   --> sysCall_sensing
             .
             .
             .
    Simulation step end (Frame display)
Cheers

Post Reply