zmqRemoteApi/clients/python/pController.py at master · CoppeliaRobotics/zmqRemoteApi · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
# Make sure to have the add-on "ZMQ remote API" running in
# CoppeliaSim. Do not launch simulation, but run this script

import math

from coppeliasim_zmqremoteapi_client import RemoteAPIClient

print('Program started')

maxForce = 100

client = RemoteAPIClient()
sim = client.require('sim')

sim.loadScene(sim.getStringParam(sim.stringparam_scenedefaultdir) + '/messaging/pControllerViaRemoteApi.ttt')

def moveToAngle(targetAngle):
    global jointAngle
    while abs(jointAngle - targetAngle) > 0.1 * math.pi / 180:
        vel = computeTargetVelocity(jointAngle, targetAngle)
        sim.setJointTargetVelocity(jointHandle, vel)
        sim.setJointTargetForce(jointHandle, maxForce, False)
        sim.step()
        jointAngle = sim.getJointPosition(jointHandle)


def computeTargetVelocity(jointAngle, targetAngle):
    dynStepSize = 0.005
    velUpperLimit = 360 * math.pi / 180
    PID_P = 0.1
    errorValue = targetAngle - jointAngle
    sinAngle = math.sin(errorValue)
    cosAngle = math.cos(errorValue)
    errorValue = math.atan2(sinAngle, cosAngle)
    ctrl = errorValue * PID_P

    # Calculate the velocity needed to reach the position
    # in one dynamic time step:
    velocity = ctrl / dynStepSize
    if velocity > velUpperLimit:
        velocity = velUpperLimit

    if velocity < -velUpperLimit:
        velocity = -velUpperLimit

    return velocity


jointHandle = sim.getObject('/Cuboid[0]/joint')
jointAngle = sim.getJointPosition(jointHandle)
sim.setJointTargetVelocity(jointHandle, 360 * math.pi / 180)

# enable the stepping mode on the client:
sim.setStepping(True)

sim.startSimulation()

moveToAngle(45 * math.pi / 180)
moveToAngle(90 * math.pi / 180)
moveToAngle(-89 * math.pi / 180)  # no -90, to avoid passing below
moveToAngle(0 * math.pi / 180)

sim.stopSimulation()

print('Program ended')