def computePoseReward(self):
totalQuatDistance = 0
for i in [0]:
rotation1 = self.targetPose[i:i+4]
rotation2 = self.agentPose[i:i+4]
diffQuat = p.getDifferenceQuaternion(rotation1,rotation2)
_, quatMag = p.getAxisAngleFromQuaternion(diffQuat)
# value is rounded because the calculations even for the same quaternions sometimes produce small errors
totalQuatDistance += np.around(quatMag, decimals=2)
return np.exp(-2*totalQuatDistance) # original value is -2*distance
def pdControlMove(self,
pd_func,
targetPositions=None,
targetVelocities=None):
numJoints = p.getNumJoints(self.id)
if targetPositions is None: # default sin move
targetPositions = []
for pos in self.getSinTargetPositions():
axis, ang = p.getAxisAngleFromQuaternion(pos)
targetPositions.append(np.array(axis) * ang)
if targetVelocities is None:
targetVelocities = np.full((numJoints, 3), 0.)
curPositions = []
curVelocities = []
for jointState in p.getJointStatesMultiDof(self.id, self.jointIndices):
state = list(jointState)
axis, ang = p.getAxisAngleFromQuaternion(state[0])
curPositions.append(np.array(ang * np.array(axis)))
curVelocities.append(np.array(state[1]))
torques = []
for i in range(numJoints):
torques.append(
pd_func(curPositions[i], curVelocities[i], targetPositions[i],
targetVelocities[i]))
p.setJointMotorControlMultiDofArray(self.id,
self.jointIndices,
p.TORQUE_CONTROL,
forces=torques)
p.stepSimulation()
def calculateAngularVelocity(self, ornStart, ornEnd, deltaTime):
dorn = p.getDifferenceQuaternion(ornStart, ornEnd)
axis, angle = p.getAxisAngleFromQuaternion(dorn)
angVel = [(x*angle) / deltaTime for x in axis]
return angVel
def pbvs(self,
arm,
goal_pos,
goal_rot,
kv=1.3,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
vs_rate=20,
plot_pose=False,
print_err=False,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.1,
perturb_Jac_joint_sigma=0.1,
perturb_orientation=False,
mu_R=0.4,
sigma_R=0.4,
perturb_motion_R_mu=0.0,
perturb_motion_R_sigma=0.0,
perturb_motion_t_mu=0.0,
perturb_motion_t_sigma=0.0,
plot_result=False,
pf_mode=False):
# Initialize variables
self.perturb_Jac_joint_mu = perturb_Jac_joint_mu
self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma
self.perturb_R_mu = mu_R
self.perturb_R_sigma = sigma_R
if arm == "right":
tool = self.right_tool
elif arm == "left":
tool = self.left_tool
else:
print('''arm incorrect, input "left" or "right" ''')
return
# Initialize pos_plot_id for later use
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot))
# record end effector pose and time stamp at each iteration
if plot_result:
start = time.time()
times = []
eef_pos = []
eef_rot = []
if pf_mode:
motion = sp.SE3()
##### Control loop starts #####
while True:
# If using particle filter and the hog computation has finished, exit pbvs control
# and assign total estimated motion in eef frame
if pf_mode and self.shared_pf_fin.value == 1:
# eef Motion estimated from pose estimate of last iteration and current fk
motion_truth = (self.Trc *
self.cur_pose_est).inverse() * get_pose(
self.robot, tool, SE3=True)
# print("motion truth:", motion_truth)
dR = sp.SO3.exp(
np.random.normal(perturb_motion_R_mu,
perturb_motion_R_sigma, 3)).matrix()
dt = np.random.normal(perturb_motion_t_mu,
perturb_motion_t_sigma, 3)
# self.draw_pose_cam(motion)
self.motion = motion_truth * sp.SE3(dR, dt)
return
# get current eef pose in camera frame
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot),
uids=pos_plot_id)
if plot_result:
eef_pos.append(cur_pos)
eef_rot.append(cur_rot)
times.append(time.time() - start)
cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot)
# Pose of goal in camera frame
pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv,
goal_pos, goal_rot)
# Evaluate current translational and rotational error
err_pos = np.linalg.norm(pos_cg)
err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1])
if err_pos < eps_pos and err_rot < eps_rot:
p.removeAllUserDebugItems()
break
else:
if print_err:
print("Error to goal, position: {:2f}, orientation: {:2f}".
format(err_pos, err_rot))
Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3)
# Perturb Rsc in SO(3) by a random variable in tangent space so(3)
if perturb_orientation:
dR = sp.SO3.exp(
np.random.normal(self.perturb_R_mu, self.perturb_R_sigma,
3))
Rsc = Rsc.dot(dR.matrix())
twist_local = np.hstack(
(np.array(pos_cg) * kv,
np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1)
local2global = np.block([[Rsc, np.zeros((3, 3))],
[np.zeros((3, 3)), Rsc]])
twist_global = local2global.dot(twist_local)
start_loop = time.time()
self.cartesian_vel_control(arm,
np.asarray(twist_global),
1 / vs_rate,
perturb_jacobian=perturb_jacobian)
if pf_mode:
delta_t = time.time() - start_loop
motion = motion * sp.SE3.exp(twist_local * delta_t)
# self.draw_pose_cam(motion)
self.goal_reached = True
print("PBVS goal achieved!")
if plot_result:
eef_pos = np.array(eef_pos)
eef_rot_rpy = np.array(
[p.getEulerFromQuaternion(quat) for quat in eef_rot])
fig, axs = plt.subplots(3, 2)
sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']]
fig.suptitle(
"Position Based Visual Servo End Effector Pose - time plot")
for i in range(3):
axs[i, 0].plot(times, eef_pos[:, i] * 100)
axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100)
axs[i, 0].legend([sub_titles[i][0], 'goal'])
axs[i, 0].set_xlabel('Time(s)')
axs[i, 0].set_ylabel('cm')
goal_rpy = p.getEulerFromQuaternion(goal_rot)
for i in range(3):
axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi)
axs[i, 1].plot(times,
goal_rpy[i] * np.ones(len(times)) * 180 / np.pi)
axs[i, 1].legend([sub_titles[i][1], 'goal'])
axs[i, 1].set_xlabel('Time(s)')
axs[i, 1].set_ylabel('deg')
for ax in axs.flat:
ax.set(xlabel='time')
plt.show()
def quat2se3(quat):
axis, angle = p.getAxisAngleFromQuaternion(quat)
return np.array(axis) * angle
def quat2w(q):
ax, angle = p.getAxisAngleFromQuaternion(q)
return np.array(ax) * angle
def quat2axis_angle(q):
return pb.getAxisAngleFromQuaternion(q)