I want to control a robot using Position Control and at the same time restrict the joint velocities to their maximum values.

I need to control all joints at once so using

**p.setJointMotorControlArray()**is preferable instead of

**p.setJointMotorControl2()**

Since

**p.setJointMotorControlArray()**does not have a "maxVelocity" parameter, this is what I'm doing to take max velocities into account:

Code: Select all

```
target_joint_positions = [x, y, z]
current_joint_positions = [x', y', z']
joint_velocity_limits = [a, b, c]
# Calculate relative positions
joint_position_difference = target_joint_positions - current_joint_positions
# Calculate the time needed to complete the motion -- This will determine how many times to call p.stepSimulation() (1/240 of a second)
max_total_time = max(joint_positions_difference / joint_velocity_limits)
# Number of times to call p.stepSimulation
num_timesteps = ceil( max_total_time / (1/240) )
# Divide the motion into equal steps that will each take less than or 1/240 th of a second
delta_joint_positions = joint_positions_difference / num_timesteps
# Make the robot move to the target positions in an additive manner
for t in range(1, num_timesteps+1):
joint_positions = current_joint_positions + delta_joint_positions * t
p.setJointMotorControlArray(id, [0,1,2], p.POSITION_CONTROL, targetPositions=joint_positions)
# Step the simulation by 1/240 seconds
p.stepSimulation()
time.sleep(1/240)
```

However, When I switch on

**p.setRealTimeSimulation(1)**, It completes the motion.

Is there something I haven't understood about

**p.stepSimulation()**? What could be wrong?

Thanks!