def get_quaternion_waypoints(point, start_quat, end_quat, step_size=np.pi/16):
angle = quat_angle_between(start_quat, end_quat)
for t in np.arange(0, angle, step_size):
fraction = t/angle
quat = p.getQuaternionSlerp(start_quat, end_quat, interpolationFraction=fraction)
#quat = quaternion_slerp(start_quat, end_quat, fraction=fraction)
yield (point, quat)
yield (point, end_quat)
def main():
val = vs()
# Set val initial pose
# high cond number
# init_pos = [[0.2]] * len(val.left_arm_joint_indices)
init_pos2 = []
for joint in val.left_arm_joints:
init_pos2.append([val.home[joint]])
print(init_pos2)
init_pos = [[0.3]] * len(val.left_arm_joint_indices)
p.resetJointStatesMultiDof(val.robot,
jointIndices=val.left_arm_joint_indices,
targetValues=init_pos2)
time.sleep(0.5)
# Optional cartesian velocity controller test
val.cart_vel_linear_test()
val.cart_vel_angular_test()
# Get Jacobian testing script
jac_trn, jac_rot = get_jacobian(val.robot, val.left_tool)
jac = get_arm_jacobian(val.robot, "left", val.left_tool)
# Get pose testing script
pos, rot = get_pose(val.robot, val.left_tool)
print("axis angle result: {}".format(rot))
# Define and draw goal pose
cur_pos, cur_rot = get_pose(val.robot, val.left_tool)
goal_pos = tuple(
np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1])))
goal_rot = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos, goal_rot)
# camera test
camera_test()
val.pbvs("left",
goal_pos,
goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.2,
perturb_Jac_joint_sigma=0.2,
perturb_orientation=False,
mu_R=0.3,
sigma_R=0.3,
plot_result=True)
p.disconnect()
def interpolate_poses(pose1, pose2, pos_step_size=0.01, ori_step_size=np.pi/16):
pos1, quat1 = pose1
pos2, quat2 = pose2
num_steps = int(math.ceil(max(get_distance(pos1, pos2)/pos_step_size,
quat_angle_between(quat1, quat2)/ori_step_size)))
for i in range(num_steps):
fraction = float(i) / num_steps
pos = (1-fraction)*np.array(pos1) + fraction*np.array(pos2)
quat = p.getQuaternionSlerp(quat1, quat2, interpolationFraction=fraction)
#quat = quaternion_slerp(quat1, quat2, fraction=fraction)
yield (pos, quat)
yield pose2
def interpolate_poses_by_num_steps(pose1, pose2, num_steps=5):
pos1, quat1 = pose1
pos2, quat2 = pose2
for i in range(num_steps):
fraction = float(i) / num_steps
pos = (1 - fraction) * np.array(pos1) + fraction * np.array(pos2)
quat = p.getQuaternionSlerp(quat1,
quat2,
interpolationFraction=fraction)
#quat = quaternion_slerp(quat1, quat2, fraction=fraction)
yield (pos, quat)
yield pose2
#getQuaternionSlerp
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
#print("duration=",frameData[0])
#print(pos=[frameData])
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
#pre-rotate to make z-up
y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
# once=False
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
chestRot = p.getQuaternionSlerp(chestRotStart,chestRotEnd,frameFraction)
neckRotStart = [frameData[13],frameData[14],frameData[15],frameData[12]]
neckRotEnd= [frameDataNext[13],frameDataNext[14],frameDataNext[15],frameDataNext[12]]
neckRot = p.getQuaternionSlerp(neckRotStart,neckRotEnd,frameFraction)
while (p.isConnected()):
target_frame = p.readUserDebugParameter(frame_id)
cur_frame = int(np.minimum(target_frame, nframe - 1))
next_frame = int(np.minimum(cur_frame + 1, nframe - 1))
frac = target_frame - cur_frame
cur_basePos = np.array(sample_data['pos']['base'][cur_frame])
next_basePos = np.array(sample_data['pos']['base'][next_frame])
target_basePos = cur_basePos + frac * (next_basePos - cur_basePos)
for i in range(len(joint_list)):
cur_orn = sample_data['orn'][joint_list[i]][cur_frame]
next_orn = sample_data['orn'][joint_list[i]][next_frame]
if i in JOINT_SPHERICAL:
target_orn[i] = p.getQuaternionSlerp(cur_orn, next_orn, frac)
elif i in JOINT_REVOLUTE:
target_orn[i] = cur_orn + frac * (next_orn - cur_orn)
print("{}".format(target_basePos))
p.resetBasePositionAndOrientation(humanoid_id, target_basePos,
target_orn[0])
for i in spherical_ind:
p.resetJointStateMultiDof(humanoid_id, i, targetValue=target_orn[i])
for i in revolute_ind:
th = target_orn[i]
p.resetJointState(humanoid_id, i, targetValue=th)
p.stepSimulation()
def main():
val = vs()
# Set val initial pose
init_pos = []
for joint in val.left_arm_joints:
init_pos.append([val.home[joint]])
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
time.sleep(0.5)
# joint velocity controller test - linear velocity
print("joint velocity controller test - linear velocity")
val.cart_vel_linear_test(t=1.5, v=0.05)
# joint velocity controller test - angular velocity
print("joint velocity controller test - angular velocity")
val.cart_vel_angular_test(t=1.5, w=0.25)
# Define and draw goal pose
cur_pos, cur_rot = get_pose(val.robot, val.left_tool)
goal_pos_global = tuple(np.asarray(np.array(cur_pos) + np.array([0.05, 0.05, -0.1])))
goal_rot_global = p.getQuaternionSlerp(cur_rot, (0, 0, 0, 1), 0.4) # weird thing happens if it is 0.7
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# transform goal_pose to camera frame
goal_pos, goal_rot = SE32_trans_rot(val.Trc.inverse() * trans_rot2SE3(goal_pos_global, goal_rot_global))
# Position Based Visual Servoing with no perturbation
print("Position Based Visual Servoing with no perturbation")
val.pbvs("left", goal_pos, goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=False,
perturb_orientation=False,
plot_result=True)
# reset to home pose
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# Position Based Visual Servoing with perturbation in Jacobian
print("Position Based Visual Servoing with perturbation in Jacobian")
val.pbvs("left", goal_pos, goal_rot,
kv=1.0,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
plot_pose=True,
perturb_jacobian=True,
perturb_Jac_joint_mu=0.2,
perturb_Jac_joint_sigma=0.2,
perturb_orientation=False,
mu_R=0.3,
sigma_R=0.3,
plot_result=True)
# reset to home pose
p.resetJointStatesMultiDof(val.robot, jointIndices=val.left_arm_joint_indices, targetValues=init_pos)
draw_pose(cur_pos, cur_rot)
draw_pose(goal_pos_global, goal_rot_global)
# execute Particle-Filter based PBVS, using default PBVS parameters which can be changed
val.pbvs_pf("left", goal_pos, goal_rot)
p.disconnect()
#getQuaternionSlerp
frameData = motion_dict['Frames'][frame]
frameDataNext = motion_dict['Frames'][frameNext]
#print("duration=",frameData[0])
#print(pos=[frameData])
basePos1Start = [frameData[1],frameData[2],frameData[3]]
basePos1End = [frameDataNext[1],frameDataNext[2],frameDataNext[3]]
basePos1 = [basePos1Start[0]+frameFraction*(basePos1End[0]-basePos1Start[0]),
basePos1Start[1]+frameFraction*(basePos1End[1]-basePos1Start[1]),
basePos1Start[2]+frameFraction*(basePos1End[2]-basePos1Start[2])]
baseOrn1Start = [frameData[5],frameData[6], frameData[7],frameData[4]]
baseOrn1Next = [frameDataNext[5],frameDataNext[6], frameDataNext[7],frameDataNext[4]]
baseOrn1 = p.getQuaternionSlerp(baseOrn1Start,baseOrn1Next,frameFraction)
#pre-rotate to make z-up
if (useZUp):
y2zPos=[0,0,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid, basePos,baseOrn)
y2zPos=[0,2,0.0]
y2zOrn = p.getQuaternionFromEuler([1.57,0,0])
basePos,baseOrn = p.multiplyTransforms(y2zPos, y2zOrn,basePos1,baseOrn1)
p.resetBasePositionAndOrientation(humanoid2, basePos,baseOrn)
chestRotStart = [frameData[9],frameData[10],frameData[11],frameData[8]]
chestRotEnd = [frameDataNext[9],frameDataNext[10],frameDataNext[11],frameDataNext[8]]
def slerp(q0, q1, t):
return pb.getQuaternionSlerp(q0, q1, t)
