How to control the position while maintaining a constant speed

asked 2021-11-08 03:09:46 -0600

aip gravatar image

UR3e is controlled using ROS. I would like to operate while giving a minute displacement of the joint angle based on the position and posture based on the following formula. (As if controlling the speed in a pseudo manner)

Δq = J-1*Δx

The way I know to give the UR3e a joint angle is "Joint Trajectory". However, if a minute displacement is given by this method, the operation will stop once, and the acceleration will become a huge number, resulting in an error. I would like to know if there is a way to keep giving a minute joint angle at a constant speed.

g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
g.trajectory.points = []
for i in range(40):
    Q = move_group.get_current_joint_values()
    jacobian =np.array(move_group.get_jacobian_matrix(Q))
    J_inv = np.linalg.inv(jacobian)
    delta_x = 0.004*0.1/4
    delta_theta = 2*pi*0.1/4
    delta_X = np.array([[0],[delta_x],[0],[0],[delta_theta],[0]])
    deltaQ = np.dot(J_inv ,delta_X)
    delta_Q = [deltaQ[0,0],deltaQ[1,0],deltaQ[2,0],deltaQ[3,0],deltaQ[4,0],deltaQ[5,0]]
    b = [Q[0]+delta_Q[0],Q[1]+delta_Q[1],Q[2]+delta_Q[2],Q[3]+delta_Q[3],Q[4]+delta_Q[4],Q[5]+delta_Q[5]]
    g.trajectory.points.append(
        JointTrajectoryPoint(positions=b, velocities=[0]*6, time_from_start=rospy.Duration(0.1)))

    client.send_goal(g)
    try:
        client.wait_for_result()
    except KeyboardInterrupt:
        client.cancel_goal()
        raise

[ERROR] [1636347379.469793187]: Trajectory message contains waypoints that are not strictly increasing in time.

edit retag flag offensive close merge delete

Comments

The way I know to give the UR3e a joint angle is "Joint Trajectory"

quick comment (and no guarantees as to stability or control performance): have you tried using a JointGroupVelocityController or JointGroupPositionController (if you really want to use position control for this)?

(As if controlling the speed in a pseudo manner)

Why not use velocity control?

gvdhoorn gravatar image gvdhoorn  ( 2021-11-08 03:42:42 -0600 )edit

I have never usedJointGroupVelocityController and JointGroupPositionController. Also, I can derive an expression for Δx, but I cannot express Δv equationally. In other words, I have done it the way I know how at this point.

aip gravatar image aip  ( 2021-11-08 04:41:00 -0600 )edit