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:
This does not complete the motion. The joint angles come close to their targets, but not exactly.
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?