def get_dq(self):
# compute dx of object
dx_xyzquat = self.get_dx()
# get euler angles from quaternion
dx_rpy = T.quat_to_euler(dx_xyzquat[3:7])
# get (6x1) dx
dx = np.hstack([dx_xyzquat[:3], dx_rpy])
# scale down dx
dx[:3] = self.move_speed * dx[:3]
dx[3:] = self.rotate_speed * dx[3:]
# compute (6xn) Jacobian with end-effector as object
J = self.compute_object_jacobian()
# compute (nx6) pseudoinverse of Jacobian
Jinv = np.linalg.pinv(J)
# compute change in configuration
dq = self.matrixMultiply(Jinv, dx) # (nx1) = (nx6) * (6x1)
# compute ik solution by from current joint states and dq
# note this ik solution does not accurately reflect arm that should stay stationary
ik_solution = self.robot_jpos_getter() + dq
# compute arm targets for stationary arms
target_left_pos = self.curr_left_pos + np.zeros_like(dx[:3])
target_left_quat_diff = T.quat_multiply(
T.quat_inverse(self.curr_left_quat), self.curr_left_quat)
target_left_quat = T.quat_multiply(self.curr_left_quat,
target_left_quat_diff)
target_right_pos = self.curr_right_pos + np.zeros_like(dx[:3])
target_right_quat_diff = T.quat_multiply(
T.quat_inverse(self.curr_right_quat), self.curr_right_quat)
target_right_quat = T.quat_multiply(self.curr_right_quat,
target_right_quat_diff)
# use inverse kinematics function to compute ik solution
ik_solution_stationary = self.inverse_kinematics(
target_right_pos,
target_right_quat,
target_left_pos,
target_left_quat,
rest_poses=self.robot_jpos_getter())
# merge two ik solutions; ik_solution for arm that moves, ik_solution_stationary for arm that does not
if self.control_arm == "left":
control_command = np.hstack(
[ik_solution_stationary[:7], ik_solution[7:]])
else: # self.control_arm == "right":
control_command = np.hstack(
[ik_solution[:7], ik_solution_stationary[7:]])
return control_command
def get_target_gripper_pose(self, target_object_pos, target_object_quat):
# compute difference between current end-effector and object poses
diff_pos = self.curr_pos - self.object_curr_pos # points from object to end-effector
diff_quat = T.quat_multiply(T.quat_inverse(self.curr_quat),
self.object_curr_quat)
diff = np.hstack([diff_pos, diff_quat])
# compute target for end-effector based on target for object and expected difference between poses
target_ee_pos = target_object_pos + diff[:3]
target_ee_quat = T.quat_multiply(target_object_quat, diff[3:7])
return (target_ee_pos, target_ee_quat)
def potential(self):
# check if goal exists
if self.goal_quat is None:
return 0
# compute difference between current and goal pose
diff_pos = self.curr_pos - self.curr_pos # no difference
diff_quat = T.quat_multiply(T.quat_inverse(self.curr_quat),
self.goal_quat)
diff = np.hstack([diff_pos, diff_quat])
# compute distance to goal
dist_pos = np.linalg.norm(diff[:3]) # no difference
dist_quat = Quaternion.distance(Quaternion(self.curr_quat),
Quaternion(self.goal_quat))
dist_quat = min(dist_quat, math.pi - dist_quat)
dist = dist_pos + dist_quat
# compute potential
pot = 0.5 * dist * dist
if self.verbose or ((not self.suppress_output) and
((self.num_iters % self.num_iters_print) == 0)):
print("BaxterRotationController: potential %f" % pot)
if self.verbose:
print(
"BaxterRotationController: position distance %f, rotation distance %f"
% (dist_pos, dist_quat))
return min(pot, self.max_potential)
def gradient(self): # compute difference between current and goal pose diff_pos = self.curr_pos - self.goal_pos diff_quat = T.quat_multiply(T.quat_inverse(self.curr_quat), self.curr_quat) # no difference diff = np.hstack([diff_pos, diff_quat]) # compute gradient grad = diff return grad
def get_dq(self): # compute dx dx = self.get_dx() # compute targets if self.control_arm == "left": # target for left arm is to reach goal pose target_left_pos = self.curr_pos + (self.move_speed * dx[:3]) # scale down position target_left_quat = T.quat_multiply(self.curr_quat, dx[3:7]) # scale down rotation not necessary, since no rotation command # target for right arm is to stay in place target_right_pos = self.curr_right_pos + np.zeros_like(dx[:3]) target_right_quat_diff = T.quat_multiply(T.quat_inverse(self.curr_right_quat), self.curr_right_quat) target_right_quat = T.quat_multiply(self.curr_right_quat, target_right_quat_diff) else: # self.control_arm == "right" # target for left arm is to stay in place target_left_pos = self.curr_left_pos + np.zeros_like(dx[:3]) target_left_quat_diff = T.quat_multiply(T.quat_inverse(self.curr_left_quat), self.curr_left_quat) target_left_quat = T.quat_multiply(self.curr_left_quat, target_left_quat_diff) # target for right arm is to reach goal pose target_right_pos = self.curr_pos + (self.move_speed * dx[:3]) # scale down position target_right_quat = T.quat_multiply(self.curr_quat, dx[3:7]) # scale down rotation not necessary, since no rotation command # use inverse kinematics function to compute dq dq = self.inverse_kinematics( target_right_pos, target_right_quat, target_left_pos, target_left_quat, rest_poses=self.robot_jpos_getter() ) return dq
def potential(self):
# compute difference between current and goal pose
diff_pos = self.object_curr_pos - self.object_goal_pos
diff_quat = T.quat_multiply(T.quat_inverse(self.object_curr_quat),
self.object_goal_quat)
diff = np.hstack([diff_pos, diff_quat])
# compute distance to goal
dist_pos = np.linalg.norm(diff[:3])
dist_quat = Quaternion.distance(Quaternion(self.object_curr_quat),
Quaternion(self.object_goal_quat))
dist = dist_pos + dist_quat
# compute potential
pot = 0.5 * dist * dist
print("BaxterObject6DPoseController: potential %f" % pot)
if True: #self.verbose: # TODO
print(
"BaxterObject6DPoseController: position distance %f, rotation distance %f"
% (dist_pos, dist_quat))
return min(pot, self.max_potential)
def get_dq(self):
# compute dx
dx = self.get_dx()
# compute targets
if self.control_arm == "left":
# target for left arm is to align with align pos
target_left_pos = self.curr_pos + (self.move_speed * dx[:3]
) # dx[:3] is zero vector
target_left_quat = T.quat_multiply(self.curr_quat, dx[3:7])
target_left_quat = T.quat_slerp(
self.curr_quat, target_left_quat,
self.rotate_speed) # scale down rotation
# target for right arm is to stay in place
target_right_pos = self.curr_right_pos + np.zeros_like(dx[:3])
target_right_quat_diff = T.quat_multiply(
T.quat_inverse(self.curr_right_quat), self.curr_right_quat)
target_right_quat = T.quat_multiply(self.curr_right_quat,
target_right_quat_diff)
else: # self.control_arm == "right"
# target for left arm is to stay in place
target_left_pos = self.curr_left_pos + np.zeros_like(dx[:3])
target_left_quat_diff = T.quat_multiply(
T.quat_inverse(self.curr_left_quat), self.curr_left_quat)
target_left_quat = T.quat_multiply(self.curr_left_quat,
target_left_quat_diff)
# target for right arm is to align with align pos
target_right_pos = self.curr_pos + (self.move_speed * dx[:3]
) # dx[:3] is zero vector
target_right_quat = T.quat_multiply(self.curr_quat, dx[3:7])
target_right_quat = T.quat_slerp(
self.curr_quat, target_right_quat,
self.rotate_speed) # scale down rotation
# use inverse kinematics function to compute dq
dq = self.inverse_kinematics(target_right_pos,
target_right_quat,
target_left_pos,
target_left_quat,
rest_poses=self.robot_jpos_getter())
return dq