Joint Control of Hexapod using Remote API in Python

[ravi]

Hi,
I am trying to move a Hexapod by providing all joint angles using Remote API in Python. All the 18 joints are already set to "Torque/Force Mode". "Motor enabled" is checked and Control loop is enabled. Below is the Python Code snippet-

Code: Select all

def move(self, joint_angles):
    vrep.simxPauseCommunication(self.clientID, True)
    for i in range(6):
        index = 3 * i
        vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 0], joint_angles[index + 0], vrep.simx_opmode_oneshot)
        vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 1], joint_angles[index + 1], vrep.simx_opmode_oneshot)
        vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 2], joint_angles[index + 2], vrep.simx_opmode_oneshot)
    vrep.simxPauseCommunication(self.clientID, False)
    vrep.simxSynchronousTrigger(self.clientID) # move simulation ahead one time step

angles = [10, 0, -10, 0] # define a trajectory
for angle in angles:
    joint_angles = [angle * pi / 180] * num_joints
    spider.move(joint_angles)

The move function takes all joint angles. It is supposed to set all joint angles and wait for the joints to reach the target position. The simulation time step dt is set to 1 ms using following code-

Code: Select all

vrep.simxSetFloatingParameter(self.clientID, vrep.sim_floatparam_simulation_time_step, dt, vrep.simx_opmode_oneshot)

At present, the joint movement is smooth, since it is taking time. However, it seems that simulation runs only for the first angle which is 10 degree. I want that all the joints to follow input angle trajectory, in which first one angle should be applied to all joints (and wait for the joints to reach the target) and then the second angle should be applied and so on.

[coppelia]

Hello,

you cannot know in advance when each joint will reach its target position, and for that reason you cannot simply feed a new trajectory point in each simulation step. Either you will have to wait until all joints have reached their positions, or you will have to make sure that the trajectory points you are playing-back are not too far apart, and that each joint is able to achieve the point-to-point moves within a simulation step. For that, make sure the force/torque is large enough, and the upper velocity limit is not too low.

Cheers

[ravi]

you will have to wait until all joints have reached their positions

Okay. I understand that I need to take feedback. Please see the code snippet below in which I keep on commanding all joints untill all joints reach target (with 1 degree tolerance for each joint)-

Code: Select all

def _getJointsPosition(self):
    return [vrep.simxGetJointPosition(self.clientID, self.joint_handles[i], vrep.simx_opmode_blocking)[1] for i in range(num_joints)]
        
def move(self, joint_angles):
    try:
        # 'np.allclose' returns True if two arrays are element-wise equal within a tolerance (1 degree)
        while not np.allclose(self._getJointsPosition(), joint_angles, atol=1):
            vrep.simxPauseCommunication(self.clientID, True)
            for i in range(6):
                index = 3 * i
                vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 0], joint_angles[index + 0], vrep.simx_opmode_oneshot)
                vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 1], joint_angles[index + 1], vrep.simx_opmode_oneshot)
                vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[index + 2], joint_angles[index + 2], vrep.simx_opmode_oneshot)
            vrep.simxPauseCommunication(self.clientID, False)
            vrep.simxSynchronousTrigger(self.clientID) # move simulation ahead one time step
    except:
        print('Unable set joint position')

It works but it seems that not all frames are shown in visualization. In other words the motion is not smooth.

I tried setting various simulation time steps such as 1 ms, 5 ms, 10 ms and 50 ms but the motion is not smooth. I suspect that the following while condition which is taking feedback into account is consuming lot of time-

Code: Select all

while not np.allclose(self._getJointsPosition(), joint_angles, atol=1)

What do you say? Any suggestions, please?

[coppelia]

Hard to guess what is going on. You need to check if your robot only is not smooth, or if another robot in the scene might also not be smoothly moving. That will tell you that simulation steps are not evenly triggered.

Cheers

[ravi]

Hi,

To figure out the issue, I did following modification in the code-

1. Removed previous wait condition i.e., while not np.allclose(self._getJointsPosition(), joint_angles, atol=1)
2. Added new wait condition i.e., while wait_for_seconds(10)

Each call to the function wait_for_seconds(n) returns true until n seconds are elasped and then returns false. Hence while loop keeps on commanding all joints for n seconds.

Well, I noticed that the spider movement is really smooth now. I dig more into the while not np.allclose(self._getJointsPosition(), joint_angles, atol=1) condition and divided the condition in while statement into two parts as following-

1. current_joints_position = self._getJointsPosition()
2. np.allclose(current_joints_position, joint_angles, atol=1)

Later I measured the time taken by each statement listed above. Below is the result of my analysis-

1. current_joints_position = self._getJointsPosition() took 180 ms approximately
2. np.allclose(current_joints_position, joint_angles, atol=1) took 0.10 ms approximately

It is clear that _getJointsPosition is the culprit. Since it is taking 180 ms in each iteration.

After close ispection, I thought that vrep.simx_opmode_blocking in _getJointsPosition function can be chnaged to vrep.simx_opmode_oneshot as following-

Code: Select all

def _getJointsPosition(self):
    return [vrep.simxGetJointPosition(self.clientID, self.joint_handles[i], vrep.simx_opmode_oneshot)[1] for i in range(num_joints)]

The motion is smooth now but it just follows only initial few trajectory points and doesn't follow all trajectory points.

[coppelia]

You cannot use simx_opmode_oneshot mode when trying to retrieve values! Make sure to understand the different modes from this page.

You rather need to stream the joint values with simx_opmode_streaming and simx_opmode_buffer.

Cheers

[ravi]

Thanks, it worked. I wasn't aware of streaming and buffer mode. Thanks again.