Issues with velocity control
Posted: Tue May 21, 2019 9:36 pm
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.
The code to set the velocity is also very simple:
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.
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)
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)