def transform_end_effector(pose, extra_pose, rot_type='quaternion'):
"""
Transform end effector pose
pose: current pose [x, y, z, ax, ay, az, w]
extra_pose: additional transformation [x, y, z, ax, ay, az, w]
matrix: if true: return (translation, rotation matrix)
else: return translation + quaternion list
"""
extra_translation = np.array(extra_pose[:3]).reshape(3, 1)
extra_rot = tr.vector_to_pyquaternion(extra_pose[3:]).rotation_matrix
c_trans = np.array(pose[:3]).reshape(3, 1)
c_rot = tr.vector_to_pyquaternion(pose[3:]).rotation_matrix
# BE CAREFUL!! Pose from KDL is ax ay az aw
# Pose from IKfast is aw ax ay az
n_trans = np.matmul(c_rot, extra_translation) + c_trans
n_rot = np.matmul(c_rot, extra_rot)
if rot_type == 'matrix':
return n_trans.flatten(), n_rot
quat_rot = np.roll(
pyquaternion.Quaternion(matrix=n_rot).normalised.elements, -1)
if rot_type == 'euler':
euler = np.array(tr.euler_from_quaternion(quat_rot, axes='rxyz'))
return np.concatenate((n_trans.flatten(), euler))
elif rot_type == 'quaternion':
return np.concatenate((n_trans.flatten(), quat_rot))
def circular_trajectory():
initial_q = [5.1818, 0.3954, -1.5714, -0.3902, 1.5448, -0.5056]
arm.set_joint_positions(initial_q, wait=True, t=2)
target_position = [0.20, -0.30, 0.80]
target_orienation = [0.01504, 0.01646, -0.01734, 0.9996]
target_orienation = [0.18603, 0.01902, -0.01464, 0.98225]
target_pose = target_position + target_orienation
deltax = np.array([0., 0.05, 0.1, 0., 0., 0.])
initial_pose = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True)
initial_pose = initial_pose[:3]
final_pose = target_position[:3]
target_q = transformations.vector_to_pyquaternion(target_orienation)
p1 = target_q.rotate(initial_pose - final_pose)
p2 = np.zeros(3)
steps = 20
duration = 10
traj = traj_utils.circunference(p1, p2, steps)
traj = np.apply_along_axis(target_q.rotate, 1, traj)
trajectory = traj + final_pose
for position in trajectory:
cmd = np.concatenate([position, target_orienation])
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
def convert_wrench(wrench_force, pose):
ee_transform = tr.vector_to_pyquaternion(pose[3:]).transformation_matrix
# # # Wrench force transformation
wFtS = force_frame_transform(ee_transform)
wrench = np.dot(wFtS, wrench_force)
return wrench
def quaternions_orientation_error(Qd, Qc):
"""
Calculates the orientation error between to quaternions
Qd is the desired orientation
Qc is the current orientation
both with respect to the same fixed frame
return vector part
"""
if isinstance(Qd, Quaternion) and isinstance(Qd, Quaternion):
ne = Qc.scalar * Qd.scalar + np.dot(np.array(Qc.vector).T, Qd.vector)
ee = Qc.scalar * np.array(Qd.vector) - Qd.scalar * np.array(
Qc.vector) + np.dot(skew(Qc.vector), Qd.vector)
ee *= np.sign(ne) # disambiguate the sign of the quaternion
return ee
else:
assert isinstance(Qd, (list, np.ndarray))
assert isinstance(Qc, (list, np.ndarray))
q1 = tr.vector_to_pyquaternion(Qd)
q2 = tr.vector_to_pyquaternion(Qc)
return quaternions_orientation_error(q1, q2)
def circular_trajectory2():
initial_q = [2.159, -1.8183, -1.9143, 0.4768, 2.5238, 4.6963]
# print(arm.end_effector().tolist())
arm.set_joint_positions(initial_q, wait=True, t=1)
target_position = [0.35951, -0.54521, 0.34393]
target_orienation = spalg.face_towards(target_position, arm.end_effector()[:3])[3:].tolist()
target_pose = target_position + target_orienation
deltax = np.array([0., 0.0, 0.2, 0., 0., 0.])
initial_pose_ = transformations.pose_euler_to_quaternion(target_pose, deltax, ee_rotation=True)
initial_pose = initial_pose_[:3]
final_pose = target_position[:3]
circle_orientation = [0,0,0,1.0]
target_q = transformations.vector_to_pyquaternion(circle_orientation)
p1 = target_q.rotate(initial_pose - final_pose)
p2 = np.zeros(3)
steps = 20
duration = 10
traj = traj_utils.circunference(p1, p2, steps, axes=[0,2,1])
traj = np.apply_along_axis(target_q.rotate, 1, traj)
trajectory = traj + final_pose
rot = spalg.face_towards(target_position, trajectory[0])[3:].tolist()
cmd = np.concatenate([trajectory[0], rot])
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
for position in trajectory:
rot = spalg.face_towards(target_position, position)[3:].tolist()
cmd = np.concatenate([position, rot])
cmd_q = arm._solve_ik(cmd)
if check_ik(arm.joint_angles(), cmd_q):
arm.set_target_pose(cmd, wait=True, t=(duration/steps))
from ur_gazebo.gazebo_spawner import GazeboModels
spawner = GazeboModels('ur3_gazebo')
m1 = create_gazebo_marker(initial_pose_, "base_link", marker_id="obj")
spawner.load_models([m1])
def gazebo_callback(msg):
global robot
robot = []
world = np.zeros(7)
world[6] = 1
for i, obj_name in enumerate(msg.name):
if obj_name.endswith("robot"):
pose = msg.pose[i]
robot = np.concatenate([conversions.from_point(pose.position), conversions.from_quaternion(pose.orientation)])
# subtract robot_position to camera_position
world[:3] -= robot[:3]
world[:3] = Quaternion(np.roll(robot[3:],1)).inverse.rotate(world[:3])
world[3:] = np.roll((Quaternion(axis=[1.0, 0.0, 0.0], degrees=-90).inverse * Quaternion(np.roll(world[3:],1))).elements,-1)
world[3:] = transformations.vector_from_pyquaternion(transformations.vector_to_pyquaternion([0.5,-0.5,-0.5,0.5]).inverse)
# world[3:] = np.roll((Quaternion(axis=[0.0, 0.0, 0.0], degrees=0).inverse * Quaternion(np.roll(world[3:],1))).elements,-1)
# print(world)orientation
br.sendTransform(world[:3],
world[3:],
rospy.Time.now(),
"sim_world",
"base_link")