Page 1 of 1

Why timeStep have to be this unreasonably small?

Posted: Mon Sep 02, 2019 7:22 am
by Zhiwu
Hello All,

I am using the newest released stable PyBullet (Bullet 2) to simulate a soft bender as several rigid segments connected with hinge joints and motors. The problem that I have is the timeStep has to be unreasonably small (as low as ~0.2 us) to get physically right result. I say it is unreasonably small because it is much much smaller than all the time constants in the system.

These rigid segments are fixed at one end, free on the other end. And I am using torque control for each motor, where torque is proportional to the displacement angle (spring-like) from equilibrium angle. The way I drive it is to oscillate (change) this equilibrium angle (same for every joint) with time on some frequency.

The problem that I have, for example, is if I drive this system on 1Hz with 2e-4/240 s ~ 0.83 us timeStep, the system looks right (as a driven harmonic oscillator). The fig. showing all the joint positions (angles) oscillate with time (at 1 Hz).
download (30).png
download (30).png (42.64 KiB) Viewed 8122 times

BUT if I just increase the timeStep a little bit to 5e-4/240 s ~ 2.08 us, when I plot the same thing, it is different and nonphysical.
download (31).png
download (31).png (47.73 KiB) Viewed 8122 times
It seems there are enough resolution in time domain. Can someone give some ideas on why this timestep has to be so small? I attached my codes here.

Any help would be greatly appreciated.

Best regards,
Zhiwu

zhiwuz at princeton.edu

Codes:

Code: Select all

import pybullet as p
import time
import math
import numpy as np
import datetime
import os

def create1DMultiActuators(actuatorNumber,unitMotorNumber,
                           actuatorMass,actuatorLength,
                           actuatorWidth,actuatorThickness,
                           basePosition=[0,0,0],baseOrientation=[0,0,0,1]):

    N=actuatorNumber;
    m=unitMotorNumber;
    thickness=actuatorThickness;
    width=actuatorWidth;
    linkLength=actuatorLength/m;
    jointLength=0.5*linkLength;
    flagLength=0.0;
    flagMass=0.0;
    
    startBoxId = p.createCollisionShape(p.GEOM_BOX,
                                        halfExtents=[0.5*jointLength, 0.5*width, 0.5*thickness]);
    linkBoxId = p.createCollisionShape(p.GEOM_BOX,
                                       halfExtents=[0.5*linkLength, 0.5*width, 0.5*thickness],
                                       collisionFramePosition=[0.5*linkLength,0,0]);
    endBoxId = p.createCollisionShape(p.GEOM_BOX,
                                      halfExtents=[0.5*jointLength, 0.5*width, 0.5*thickness],
                                      collisionFramePosition=[0.5*jointLength,0,0]);

    flagBoxId = p.createCollisionShape(p.GEOM_BOX,
                                      halfExtents=[0.5*flagLength, 0.5*width, 0.5*thickness],
                                      collisionFramePosition=[0.5*flagLength,0,0]);

        
    mass = actuatorMass/(2*m);
    visualShapeId = -1;
    basePosition=basePosition;
    baseOrientation = baseOrientation;

    link_Masses = [actuatorMass/m for i in range(N*m-1)]
    link_Masses.append(actuatorMass/(2*m));
    link_Masses.append(flagMass);

    linkCollisionShapeIndices = [linkBoxId for i in range(N*m-1)]
    linkCollisionShapeIndices.append(endBoxId);
    linkCollisionShapeIndices.append(flagBoxId);

    linkVisualShapeIndices = [-1 for i in range(N*m+1)];

    linkPositions=[[0.5*jointLength, 0, 0]];
    for i in range(N*m-1):
        linkPositions.append([linkLength, 0, 0]);
    linkPositions.append([jointLength, 0, 0]);


    linkOrientations = [[0, 0, 0, 1] for i in range(N*m+1)]

    linkInertialFramePositions = [[0.5*linkLength, 0, 0] for i in range(N*m-1)]
    linkInertialFramePositions.append([0.5*jointLength, 0, 0]);
    linkInertialFramePositions.append([0.5*flagLength, 0, 0]);

    linkInertialFrameOrientations = [[0, 0, 0, 1] for i in range(N*m+1)]
    indices = [i for i in range(N*m+1)]
    jointTypes = [p.JOINT_REVOLUTE for i in range(N*m)]
    jointTypes.append(p.JOINT_FIXED);
    axis = [[0, 1, 0] for i in range(N*m+1)]

    boxId = p.createMultiBody(mass,
                              startBoxId,
                              visualShapeId,
                              basePosition,
                              baseOrientation,
                              linkMasses=link_Masses,
                              linkCollisionShapeIndices=linkCollisionShapeIndices,
                              linkVisualShapeIndices=linkVisualShapeIndices,
                              linkPositions=linkPositions,
                              linkOrientations=linkOrientations,
                              linkInertialFramePositions=linkInertialFramePositions,
                              linkInertialFrameOrientations=linkInertialFrameOrientations,
                              linkParentIndices=indices,
                              linkJointTypes=jointTypes,
                              linkJointAxis=axis);

    jointNumber=p.getNumJoints(boxId)-1;
    #Disable the default motors
    for joint in range(jointNumber):
        p.setJointMotorControl2(boxId,
                                joint,
                                p.VELOCITY_CONTROL,
                                force=0)
    return [boxId, jointNumber];


def generate1DMotorVoltages(actuatorVoltages,actuatorNumber,unitMotorNumber):
    motorVoltages=[];
    N=actuatorNumber;
    m=unitMotorNumber;
    for i in range(N):
        for j in range(m):
            motorVoltages.append(actuatorVoltages[i]/m);

    return motorVoltages;
actuator1DDensity=0.4806;
actuatorLength=10;
actuatorMass=actuator1DDensity*actuatorLength;

drivenFrequency=1.0; #in Hz
N=1;
m=5;
thickness=0.1;
width=3;
simTime=0;


timeStep=(2e-4)/240;
dampingEta=0.4;
simCycles=int(2.4e6);
recordStepInterval=5;


dataTime=[];
dataTheta=[];
dataAngularVelocities=[];
dataTor=[];
dataPositions=[];
dataMotorVoltages=[];


linkLength=actuatorLength/m;
jointLength=0.5*linkLength;

p.connect(p.DIRECT)
p.resetSimulation()

def TorVolThe(Theta,angularVelocity,Voltage):
    thetaTarget=-1.5e-3*Voltage;
    if m==1:
        K=0.7796*1e5;
    if m==2:
        K=1.5735*1e5;
    if m==4:
        K=3.1630*1e5;
    else:
        K=1.5735*1e5*(0.5*m);
    omega=2*math.pi*drivenFrequency;
    #omega=1;
    Tor=-width*K*((Theta-thetaTarget)+(dampingEta/omega)*angularVelocity);
    return Tor



(boxId,jointNumber)=create1DMultiActuators(N,m,actuatorMass,actuatorLength,width,thickness);

p.setTimeStep(timeStep);
p.setGravity(0, 0, 0)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, 
                 linearDamping=0.0, angularDamping=0.0,jointDamping=0.0)

cid = p.createConstraint(boxId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 0])

for step in range(simCycles):
    if (step+1)%5000==0:
        print('N=',N,', m=',m,"Frequency=",round(drivenFrequency,2),"Hz, step=",step+1);

    simTime=step*timeStep;

    theta=[];
    angularVelocities=[];
    positions=[];
    for joint in range(jointNumber):
        theta.append(p.getJointState(boxId,joint)[0]);
        angularVelocities.append(p.getJointState(boxId,joint)[1]);
        positions.append(p.getLinkState(boxId,joint)[4]);
    positions.append(p.getLinkState(boxId,jointNumber)[4]); #append the flag position

    actuatorVoltages=[200*math.cos(2*math.pi*drivenFrequency*simTime)];
    motorVoltages=generate1DMotorVoltages(actuatorVoltages, N, m);

    Tor=[TorVolThe(theta[joint],angularVelocities[joint],motorVoltages[joint]) for joint in range(jointNumber)]


    if (step+1)%recordStepInterval==0 or step==0 or step==simCycles-1:
        dataTime.append(simTime);
        dataTheta.append(theta);
        dataAngularVelocities.append(angularVelocities);
        dataPositions.append(positions);
        dataMotorVoltages.append(motorVoltages);
        dataTor.append(Tor);

    for joint in range(jointNumber):
        p.setJointMotorControl2(boxId,
                               joint,
                               p.TORQUE_CONTROL,
                               force=Tor[joint])

    p.stepSimulation();
    time.sleep(timeStep);

p.disconnect();
dataTime=np.array(dataTime);
dataTheta=np.array(dataTheta);
dataPositions=np.array(dataPositions);
dataTor=np.array(dataTor);
dateTime=datetime.datetime.today().strftime('%m_%d_%Y_%H_%M');
filename='AC_Transient_ZZ5_damp_N_'+str(N)+'_m_'+str(m)+'_Frequency_'+str(round(drivenFrequency,2))+'Hz_'+'DampingEta_'+str(round(dampingEta,2))+'_'+dateTime+'.npz';
outfile='data/'+filename;

np.savez(outfile,dataTime=dataTime,dataTheta=dataTheta,dataAngularVelocities=dataAngularVelocities,
         dataPositions=dataPositions,dataMotorVoltages=dataMotorVoltages,dataTor=dataTor, timeStep=timeStep, 
         simCycles=simCycles, recordStepInterval=recordStepInterval, N=N, m=m, drivenFrequency=drivenFrequency);
         
         

import sys
import math
import numpy as np
import os
import matplotlib
import matplotlib.pyplot as plt


print(filename)
loadFile=filename;
npzFile=np.load('data/'+loadFile);
N=npzFile['N'];
m=npzFile['m'];
dataTime=npzFile['dataTime'];
dataTheta=npzFile['dataTheta'];
dataPositions=npzFile['dataPositions'];
dataTor=npzFile['dataTor'];
dataMotorVoltages=npzFile['dataMotorVoltages'];
timeStep=npzFile['timeStep'];
simCycles=npzFile['simCycles'];
recordStepInterval=npzFile['recordStepInterval'];
plt.figure(figsize=(20,15))
for i in range(dataTheta.shape[1]):
    plt.plot(dataTime,-dataTheta[:,i],'o-',label='theta'+str(i+1)+'-data')
plt.xlabel("Time (s)")
plt.ylabel("Theta (rad)")
plt.title(loadFile+'_'+'theta-time')
plt.legend()
plt.show()


Re: Why timeStep have to be this unreasonably small?

Posted: Thu Sep 05, 2019 2:27 am
by steven
hi Could you tell me the bullet version you used? i am testing this case with the attached code but get a different result.
my bullet version is 2.5.5.

Re: Why timeStep have to be this unreasonably small?

Posted: Fri Sep 06, 2019 4:59 am
by Zhiwu
steven wrote: Thu Sep 05, 2019 2:27 am hi Could you tell me the bullet version you used? i am testing this case with the attached code but get a different result.
my bullet version is 2.5.5.
Hi Steven,

Thank you for your reply. Actually I found that it was my fault. I am using global variable m=5 for the figs but I put m=8 in the codes. (I will pay attention to that when I post a question next time.)

In the original post, I corrected the codes posted, tested it on my computer and attached the figs I got. The results are fundamentally still the same, and so the question remains.

If I used

Code: Select all

pip show pybullet 
in the terminal, the version it gives me is 2.5.5. If I used

Code: Select all

import pybullet as p
p.connect(p.DIRECT);
print(p.getAPIVersion)
p.disconnect();

in python, the output is "201908110".

Best regards,
Zhiwu

Re: Why timeStep have to be this unreasonably small?

Posted: Wed Sep 11, 2019 8:56 am
by steven
i change your code to use the POSITION_CONTROL mode with (5e-3)/240 us timestep which is bigger than what you used and it can work well as below.
if you have to use the TORQUE_CONTROL mode based on some other consideration, Could you explain your function of TorVolThe(Theta,angularVelocity,Voltage)? thanks.

Re: Why timeStep have to be this unreasonably small?

Posted: Sat Sep 14, 2019 8:51 pm
by Zhiwu
steven wrote: Wed Sep 11, 2019 8:56 am i change your code to use the POSITION_CONTROL mode with (5e-3)/240 us timestep which is bigger than what you used and it can work well as below.
if you have to use the TORQUE_CONTROL mode based on some other consideration, Could you explain your function of TorVolThe(Theta,angularVelocity,Voltage)? thanks.
Hi Steven,

I am trying to model dynamics of a voltage-controlled soft bending actuator (piezoelectric material bonded on non-extendable material (e.g. aluminum foil)). Because the actuator mainly bend but no compression or extension, in my model, the actuator is modeled as several rigid segments connected with joints. There are two factors combining together to decide its dynamics. The first factor is due to voltage: the joint equilibrium position (angle) will be decided by voltage assuming no other external forces. The second one is due to material properties - elastic modullus: the bender tries to go back to its equilibrium position (by having bending moment applied). I also added damping term. So in function TorVolThe(Voltage, angularVelocity, Theta), thetaTarget is the joint equilibrium position, linearly decided by applied voltage. And K is a constant per unit width deciding how large the restoring torque that try to push the bender back to the equilibrium position. K will change depending on how finely the segment is defined. (It is similar with a spring: when a spring is cut in half, the spring constant doubles.) omega is the AC driving frequency.

Tor is the torque of the motor applied. It has two terms. The first term -width*K*(Theta-thetaTarget) is a restoring torque, the torque due to the elastic property of the material pushing the bender to its equilibrium position. The second term -width*K*(dampingEta/omega)*angularVelocity is the damping term, proportional to the angularVelocity of the joint. (This is in CGS unit - centimeter, gram, second. Voltage is in kV unit.)

I have verified this model by comparing PyBullet's results with the result from an FEM software COMSOL. They match well.

I am using TORQUE_CONTROL as it is more intuitive to describe the physics for the model. POSITION_CONTROL might be? I am not so sure.

In addition, I tested POSITION_CONTROL at 5Hz with timestep 5e-3/240 s, but got a different result, see attached. Would you be able to post your POSITION_CONTROL test codes to allow me see what is going on?
download (43).png
download (43).png (51.02 KiB) Viewed 7959 times
Here is my test codes:

Code: Select all

import pybullet as p
import time
import math
import numpy as np
import datetime
import os

def create1DMultiActuators(actuatorNumber,unitMotorNumber,
                           actuatorMass,actuatorLength,
                           actuatorWidth,actuatorThickness,
                           basePosition=[0,0,0],baseOrientation=[0,0,0,1]):

    N=actuatorNumber;
    m=unitMotorNumber;
    thickness=actuatorThickness;
    width=actuatorWidth;
    linkLength=actuatorLength/m;
    jointLength=0.5*linkLength;
    flagLength=0.0;
    flagMass=0.0;
    
    startBoxId = p.createCollisionShape(p.GEOM_BOX,
                                        halfExtents=[0.5*jointLength, 0.5*width, 0.5*thickness]);
    linkBoxId = p.createCollisionShape(p.GEOM_BOX,
                                       halfExtents=[0.5*linkLength, 0.5*width, 0.5*thickness],
                                       collisionFramePosition=[0.5*linkLength,0,0]);
    endBoxId = p.createCollisionShape(p.GEOM_BOX,
                                      halfExtents=[0.5*jointLength, 0.5*width, 0.5*thickness],
                                      collisionFramePosition=[0.5*jointLength,0,0]);

    flagBoxId = p.createCollisionShape(p.GEOM_BOX,
                                      halfExtents=[0.5*flagLength, 0.5*width, 0.5*thickness],
                                      collisionFramePosition=[0.5*flagLength,0,0]);

        
    mass = actuatorMass/(2*m);
    visualShapeId = -1;
    basePosition=basePosition;
    baseOrientation = baseOrientation;

    link_Masses = [actuatorMass/m for i in range(N*m-1)]
    link_Masses.append(actuatorMass/(2*m));
    link_Masses.append(flagMass);

    linkCollisionShapeIndices = [linkBoxId for i in range(N*m-1)]
    linkCollisionShapeIndices.append(endBoxId);
    linkCollisionShapeIndices.append(flagBoxId);

    linkVisualShapeIndices = [-1 for i in range(N*m+1)];

    linkPositions=[[0.5*jointLength, 0, 0]];
    for i in range(N*m-1):
        linkPositions.append([linkLength, 0, 0]);
    linkPositions.append([jointLength, 0, 0]);


    linkOrientations = [[0, 0, 0, 1] for i in range(N*m+1)]

    linkInertialFramePositions = [[0.5*linkLength, 0, 0] for i in range(N*m-1)]
    linkInertialFramePositions.append([0.5*jointLength, 0, 0]);
    linkInertialFramePositions.append([0.5*flagLength, 0, 0]);

    linkInertialFrameOrientations = [[0, 0, 0, 1] for i in range(N*m+1)]
    indices = [i for i in range(N*m+1)]
    jointTypes = [p.JOINT_REVOLUTE for i in range(N*m)]
    jointTypes.append(p.JOINT_FIXED);
    axis = [[0, 1, 0] for i in range(N*m+1)]

    boxId = p.createMultiBody(mass,
                              startBoxId,
                              visualShapeId,
                              basePosition,
                              baseOrientation,
                              linkMasses=link_Masses,
                              linkCollisionShapeIndices=linkCollisionShapeIndices,
                              linkVisualShapeIndices=linkVisualShapeIndices,
                              linkPositions=linkPositions,
                              linkOrientations=linkOrientations,
                              linkInertialFramePositions=linkInertialFramePositions,
                              linkInertialFrameOrientations=linkInertialFrameOrientations,
                              linkParentIndices=indices,
                              linkJointTypes=jointTypes,
                              linkJointAxis=axis);

    jointNumber=p.getNumJoints(boxId)-1;
    #Disable the default motors
    for joint in range(jointNumber):
        p.setJointMotorControl2(boxId,
                                joint,
                                p.VELOCITY_CONTROL,
                                force=0)
    return [boxId, jointNumber];


def generate1DMotorVoltages(actuatorVoltages,actuatorNumber,unitMotorNumber):
    motorVoltages=[];
    N=actuatorNumber;
    m=unitMotorNumber;
    for i in range(N):
        for j in range(m):
            motorVoltages.append(actuatorVoltages[i]/m);

    return motorVoltages;
actuator1DDensity=0.4806;
actuatorLength=10;
actuatorMass=actuator1DDensity*actuatorLength;

drivenFrequency=5.0; #in Hz
N=1;
m=8;
thickness=0.1;
width=3;
simTime=0;


timeStep=(5e-3)/240;
dampingEta=0.4;
simCycles=int(2.4e5);
recordStepInterval=1;


dataTime=[];
dataTheta=[];
dataAngularVelocities=[];
dataTor=[];
dataPositions=[];
dataMotorTorques=[];
dataMotorVoltages=[];


linkLength=actuatorLength/m;
jointLength=0.5*linkLength;

p.connect(p.DIRECT)
p.resetSimulation()

def TorVolThe(Theta,angularVelocity,Voltage):
    thetaTarget=-1.5e-3*Voltage;
    if m==1:
        K=0.7796*1e5;
    if m==2:
        K=1.5735*1e5;
    if m==4:
        K=3.1630*1e5;
    else:
        K=1.5735*1e5*(0.5*m);
    omega=2*math.pi*drivenFrequency;
    #omega=1;
    Tor=-width*K*((Theta-thetaTarget)+(dampingEta/omega)*angularVelocity);
    return Tor



(boxId,jointNumber)=create1DMultiActuators(N,m,actuatorMass,actuatorLength,width,thickness);

p.setTimeStep(timeStep);
p.setGravity(0, 0, 0)
p.changeDynamics(boxId, -1, spinningFriction=0.001, rollingFriction=0.001, 
                 linearDamping=0.0, angularDamping=0.0,jointDamping=0.0)

cid = p.createConstraint(boxId, -1, -1, -1, p.JOINT_FIXED, [0, 0, 0], [0, 0, 0], [0, 0, 0])

for step in range(simCycles):
    if (step+1)%5000==0:
        print('N=',N,', m=',m,"Frequency=",round(drivenFrequency,2),"Hz, step=",step+1);

    simTime=step*timeStep;

    theta=[];
    angularVelocities=[];
    positions=[];
    motorTorques=[];
    for joint in range(jointNumber):
        theta.append(p.getJointState(boxId,joint)[0]);
        angularVelocities.append(p.getJointState(boxId,joint)[1]);
        positions.append(p.getLinkState(boxId,joint)[4]);
        motorTorques.append(p.getJointState(boxId,joint)[3]);
    positions.append(p.getLinkState(boxId,jointNumber)[4]); #append the flag position

    actuatorVoltages=[200*math.cos(2*math.pi*drivenFrequency*simTime)];
    motorVoltages=generate1DMotorVoltages(actuatorVoltages, N, m);

    targetTheta=-(1.5e-3)*200*math.cos(2*math.pi*drivenFrequency*simTime);
    Tor=[TorVolThe(theta[joint],angularVelocities[joint],motorVoltages[joint]) for joint in range(jointNumber)]


    if (step+1)%recordStepInterval==0 or step==0 or step==simCycles-1:
        dataTime.append(simTime);
        dataTheta.append(theta);
        dataAngularVelocities.append(angularVelocities);
        dataPositions.append(positions);
        dataMotorTorques.append(motorTorques);
        dataMotorVoltages.append(motorVoltages);
        dataTor.append(Tor);

    for joint in range(jointNumber):
        p.setJointMotorControl2(boxId,
                               joint,
                               p.POSITION_CONTROL,
                               targetPosition=targetTheta)

    p.stepSimulation();
    time.sleep(timeStep);

p.disconnect();
dataTime=np.array(dataTime);
dataTheta=np.array(dataTheta);
dataPositions=np.array(dataPositions);
dataMotorTorques=np.array(dataMotorTorques);
dataTor=np.array(dataTor);
dateTime=datetime.datetime.today().strftime('%m_%d_%Y_%H_%M');
filename='AC_Transient_ZZ5_damp_N_'+str(N)+'_m_'+str(m)+'_Frequency_'+str(round(drivenFrequency,2))+'Hz_'+'DampingEta_'+str(round(dampingEta,2))+'_'+dateTime+'.npz';
outfile='data/'+filename;

np.savez(outfile,dataTime=dataTime,dataTheta=dataTheta,dataAngularVelocities=dataAngularVelocities,
         dataPositions=dataPositions,dataMotorTorques=dataMotorTorques,dataMotorVoltages=dataMotorVoltages,
         dataTor=dataTor, timeStep=timeStep,simCycles=simCycles, recordStepInterval=recordStepInterval, 
         N=N, m=m, drivenFrequency=drivenFrequency);
         
         
print(filename)

import sys
import math
import numpy as np
import os
import matplotlib
import matplotlib.pyplot as plt

loadFile=filename;
npzFile=np.load('data/'+loadFile);
N=npzFile['N'];
m=npzFile['m'];
dataTime=npzFile['dataTime'];
dataTheta=npzFile['dataTheta'];
dataPositions=npzFile['dataPositions'];
dataAngularVelocities=npzFile['dataAngularVelocities'];
dataTor=npzFile['dataTor'];
dataMotorTorques=npzFile['dataMotorTorques'];
dataMotorVoltages=npzFile['dataMotorVoltages'];
timeStep=npzFile['timeStep'];
simCycles=npzFile['simCycles'];
recordStepInterval=npzFile['recordStepInterval'];
plt.figure(figsize=(20,15))
for i in range(dataTheta.shape[1]):
    plt.plot(dataTime,-dataTheta[:,i],'o-',label='theta'+str(i+1)+'-data')
plt.xlabel("Time (s)")
plt.ylabel("Theta (rad)")
plt.title(loadFile+'_'+'theta-time')
plt.legend()
plt.show()

Thanks.

Best regards,
Zhiwu

Re: Why timeStep have to be this unreasonably small?

Posted: Mon Sep 16, 2019 1:26 pm
by steven
sorry, i still don't understand why this function be written like that. i have some questions as below.
1. where do these constants come from such as -1.5e-3, 1.5735*1e5?
2. why the joint torque only have relation with the joint delta theta(if not consider the damping)?

also you can call the POSITION CONTROL like this.
p.setJointMotorControl2(boxId,
joint,
p.POSITION_CONTROL,
targetPosition=targetTheta,
force=1000000000000, positionGain=1)

thanks.

Re: Why timeStep have to be this unreasonably small?

Posted: Mon Sep 23, 2019 3:04 am
by Zhiwu
steven wrote: Mon Sep 16, 2019 1:26 pm sorry, i still don't understand why this function be written like that. i have some questions as below.
1. where do these constants come from such as -1.5e-3, 1.5735*1e5?
2. why the joint torque only have relation with the joint delta theta(if not consider the damping)?

also you can call the POSITION CONTROL like this.
p.setJointMotorControl2(boxId,
joint,
p.POSITION_CONTROL,
targetPosition=targetTheta,
force=1000000000000, positionGain=1)

thanks.
Hi Steven,
steven wrote: Mon Sep 16, 2019 1:26 pm 1. where do these constants come from such as -1.5e-3, 1.5735*1e5?
These constants come from a Finite Element Method (FEM) simulation from COMSOL. In the other simulation, materials/geometry for the real actuator is put in, and these two parameters are extracted. -1.5e-3 describes how the bending change with input voltage, and 1.5735*1e5 describes the elastic property of the actuator.
steven wrote: Mon Sep 16, 2019 1:26 pm 2. why the joint torque only have relation with the joint delta theta(if not consider the damping)?
This is from the assumption of linear elasticity. Similar to Hooke's Law, the torque is promotional to how much it displaces from the equilibrium position (angle).

(How do you think the reason for such a small time step?)

Thank you very much.

Best regards,
Zhiwu

Re: Why timeStep have to be this unreasonably small?

Posted: Wed Sep 25, 2019 3:36 am
by steven
This is from the assumption of linear elasticity. Similar to Hooke's Law, the torque is promotional to how much it displaces from the equilibrium position (angle).
i hava an intuition that the torque of joint near the base should be larger than the joint near the end given the same delta theta, right? so i make a test and change your function as below and to my surprise it can work very well. Can you help to confirm your function? thanks.

Code: Select all

timeStep=(5e-3)/240;

Code: Select all

def TorVolThe(Theta,angularVelocity,Voltage, join):
    thetaTarget=-1.5e-3*Voltage;
    if join==1:
        K=0.7796*1e5;
    if join==2:
        K=1.5735*1e5;
    if join==4:
        K=3.1630*1e5;
    else:
        K=1.5735*1e5*(0.5*join);
    omega=2*math.pi*drivenFrequency;
    #omega=1;
    Tor=-width*K*((Theta-thetaTarget)+(dampingEta/omega)*angularVelocity);
    #Tor=thetaTarget
    return Tor

Code: Select all

Tor=[TorVolThe(theta[joint],angularVelocities[joint],motorVoltages[joint], jointNumber - joint) for joint in range(jointNumber)]