Issues with velocity control

Official Python bindings with a focus on reinforcement learning and robotics.
Post Reply
timeforscience
Posts: 3
Joined: Sat Jan 12, 2019 2:11 am

Issues with velocity control

Post by timeforscience »

Hello all, I'm trying to implement velocity control in one of my simulations, but it doesn't seem to quite work. I have a 6DOF arm that I apply a velocity to the base joint and set the remaining joints to 0. The base joint moves, but the other joints sag as if there isn't enough torque to hold them up. I assumed my torque limits were the issue, but position control works fine without any sagging. Am I misunderstanding the velocity control or something?

Here are the functions I use to set joint positions and speeds. self.joint_map is used to get the joint index. I'm using the default max_force in my test.

Code: Select all

    
    def set_joint_positions(self, joint_positions: dict, speed: float):
        for joint, position in joint_positions.items():
            p.setJointMotorControl2(self.device, self.joint_map[joint], p.POSITION_CONTROL,
                                    targetPosition=position,
                                    maxVelocity=speed)

    def set_joint_speeds(self, joint_speeds: dict, max_force: float = 500.0):
        """Set the speed of each joint in rads/s"""
        for joint, velocity in joint_speeds.items():
            p.setJointMotorControl2(self.device, self.joint_map[joint], p.VELOCITY_CONTROL,
                                    targetVelocity=velocity,
                                    force=max_force)
The code to set the velocity is also very simple:

Code: Select all

    u = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0] #np.dot(J.T, v)
    signals = {name: vel for name, vel in zip(robot.JOINT_NAMES, u)}
    robot.set_joint_speeds(signals)
I've attached my urdf file as well. I'm very frustrated as I feel I'm very close, but can't quite get there. Any guidance would be appreciated.
Attachments
ur5_free_base.txt
(8.15 KiB) Downloaded 416 times
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Issues with velocity control

Post by steven »

Could you attach a complete simple test? i can debug it with you to find what happened there.
so far my understanding about the procedure:
1. apply the gravity to the multibody then will change the velocity,
2. apply the jointMotor constraint that will change the velocity back to the targetVelocity, 0 for your case,
3. update the position.

seems that it can get what you expect. i need to debug to get the more details. thanks.
timeforscience
Posts: 3
Joined: Sat Jan 12, 2019 2:11 am

Re: Issues with velocity control

Post by timeforscience »

Thanks for your response! Here is a full working example:

Code: Select all

import numpy as np
import time
import pybullet as p
import pybullet_data
import os

HOME_POSITION = [0.0, -3.14159/2., 3.14159/2., 0.0, 0, 0.0]

engine = p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"))

path = 'ur5_free_base.urdf'
filepath = path
device = p.loadURDF(filepath, [0, 0, 0],
                    [0, 0, 0, 1], flags=p.URDF_USE_SELF_COLLISION)

for i in range(6):
    p.resetJointState(device, i, HOME_POSITION[i])

p.resetBasePositionAndOrientation(device,
                                  [0, 0, 0.02],
                                  [0, 0, 0, 1])
path = 'baseplate.urdf'
filepath2 = path
baseplate = p.loadURDF(filepath2)

base_constraint = p.createConstraint(device,
                                      -1,
                                      baseplate, 0,
                                      p.JOINT_FIXED, [0, 0, 0],
                                      [0, 0, 0],
                                      [0, 0, 0])
while True:
    u = [0.5, 0.0, 0.0, 0.0, 0.0, 0.0] 
    max_force = 500
    for i, velocity in enumerate(u):
        p.setJointMotorControl2(device, i, p.VELOCITY_CONTROL,
                                targetVelocity=velocity,
                                force=max_force)
    p.stepSimulation()
    time.sleep(1/250.0)

I've also attached the baseplate urdf as I forgot that last time. I had to change the file ending from urdf to txt to upload to the site so if you want to test it don't forget to update the file name.
Attachments
baseplate.txt
(791 Bytes) Downloaded 373 times
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Issues with velocity control

Post by steven »

Could you also attach all the mesh files? thanks.

Code: Select all

b3Printf: ur5_free_base.urdf:24: cannot find 'meshes/ur5/visual/base.dae' in any directory in urdf path
steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Issues with velocity control

Post by steven »

steven
Posts: 83
Joined: Mon Nov 05, 2018 8:16 am
Location: China

Re: Issues with velocity control

Post by steven »

update the procedure of this case.
1. apply the gravity to the multibody then will change the velocity,
2. apply the constraints, including
  • 1> jointMotor constraint that will try to change the velocity back to the targetVelocity, 0 for your case;
  • 2> Fixed constraint
  • 3> contact constraint and friction constraint
3. update the position.
after step 2, we can't make sure that the joint velocity reaches its target value, maybe has some errors which will cause the joint position changed accordingly at step 3. after a long time the accumulative error will become much larger. but we can make the error be more less by the following two interfaces but it still can't fix this issue, only make it look well in a period of time.

Code: Select all

p.setPhysicsEngineParameter(numSolverIterations=200)
p.setPhysicsEngineParameter(solverResidualThreshold=1e-30)
so i dont' think we can control the position by the velocity mode.
Post Reply