Controlling a robot using Position Control, while taking into account the joint velocities

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
Dyu
Posts: 4
Joined: Sun Oct 11, 2020 1:17 pm

Controlling a robot using Position Control, while taking into account the joint velocities

Post by Dyu »

Hi,
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)

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