How to control the position while maintaining a constant speed
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.
quick comment (and no guarantees as to stability or control performance): have you tried using a
JointGroupVelocityController
orJointGroupPositionController
(if you really want to use position control for this)?Why not use velocity control?
I have never used
JointGroupVelocityController
andJointGroupPositionController
. 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.