def TransformAngularVelocityToLocalFrame(self, angular_velocity,
orientation):
"""Transform the angular velocity from world frame to robot's frame.
Args:
angular_velocity: Angular velocity of the robot in world frame.
orientation: Orientation of the robot represented as a quaternion.
Returns:
angular velocity of based on the given orientation.
"""
# Treat angular velocity as a position vector, then transform based on the
# orientation given by dividing (or multiplying with inverse).
# Get inverse quaternion assuming the vector is at 0,0,0 origin.
tr = dp.TinySpatialTransform()
tr.translation = dp.Vector3(1, 2, 3)
tr.translation.set_zero()
orn = dp.Quaternion(orientation[0], orientation[1], orientation[2],
orientation[3])
tr.rotation = dp.Matrix3(orn)
tr_inv = tr.get_inverse()
rel_vel = tr.apply_inverse(
dp.Vector3(angular_velocity[0], angular_velocity[1],
angular_velocity[2]))
#tr.print("tds trans")
#print("tds rel_vel=",rel_vel)
#print(tr)
return vec3_to_np(rel_vel)
def sync_visual_transforms(mb, b2vis, vis):
for key in b2vis:
v = b2vis[key]
#print("v.link_index=",v.link_index)
link_world_trans = mb.get_world_transform(v.link_index)
vpos = v.origin_xyz
vorn = dp.TinyQuaternion(0.0, 0.0, 0.0, 1.0)
vorn.set_euler_rpy(v.origin_rpy)
trv = dp.TinySpatialTransform()
trv.translation = vpos
trv.rotation = dp.TinyMatrix3x3(vorn)
#print("link_world_trans.x=",link_world_trans.translation.x)
#print("link_world_trans.y=",link_world_trans.translation.y)
#print("link_world_trans.z=",link_world_trans.translation.z)
gfx_world_trans = link_world_trans * trv #trvi
rot = gfx_world_trans.rotation
#print("gfx_world_trans.translation=",gfx_world_trans.translation)
transpose = False
if transpose:
mat = [[
rot.get_row(0).x,
rot.get_row(1).x,
rot.get_row(2).x, gfx_world_trans.translation.x
],
[
rot.get_row(0).y,
rot.get_row(1).y,
rot.get_row(2).y, gfx_world_trans.translation.y
],
[
rot.get_row(0).z,
rot.get_row(1).z,
rot.get_row(2).z, gfx_world_trans.translation.z
], [0., 0., 0., 1.]]
else:
mat = [[
rot.get_row(0).x,
rot.get_row(0).y,
rot.get_row(0).z, gfx_world_trans.translation.x
],
[
rot.get_row(1).x,
rot.get_row(1).y,
rot.get_row(1).z, gfx_world_trans.translation.y
],
[
rot.get_row(2).x,
rot.get_row(2).y,
rot.get_row(2).z, gfx_world_trans.translation.z
], [0., 0., 0., 1.]]
vis[v.vis_name].set_transform(np.array(mat))
def joint_angles_from_link_position(self,
robot,
link_position,
link_id,
joint_ids,
position_in_world_frame,
base_translation=(0, 0, 0),
base_rotation=(0, 0, 0, 1)):
"""Uses Inverse Kinematics to calculate joint angles.
Args:
robot: A robot instance.
link_position: The (x, y, z) of the link in the body or the world frame,
depending on whether the argument position_in_world_frame is true.
link_id: The link id as returned from loadURDF.
joint_ids: The positional index of the joints. This can be different from
the joint unique ids.
position_in_world_frame: Whether the input link_position is specified
in the world frame or the robot's base frame.
base_translation: Additional base translation.
base_rotation: Additional base rotation.
Returns:
A list of joint angles.
"""
if not position_in_world_frame:
#tds
base_world_tr = self.mb.get_world_transform(-1)
local_base_tr = dp.TinySpatialTransform()
local_base_tr.translation = dp.Vector3(base_translation[0],
base_translation[1],
base_translation[2])
local_base_tr.rotation = dp.Matrix3(
dp.Quaternion(base_rotation[0], base_rotation[1],
base_rotation[2], base_rotation[3]))
world_tr = base_world_tr * local_base_tr
local_link_tr = dp.TinySpatialTransform()
local_link_tr.rotation = dp.Matrix3(dp.Quaternion(0, 0, 0, 1))
local_link_tr.translation = dp.Vector3(link_position[0],
link_position[1],
link_position[2])
link_tr = world_tr * local_link_tr
world_link_pos = [
link_tr.translation[0], link_tr.translation[1],
link_tr.translation[2]
]
else:
world_link_pos = link_position
ik_solver = 0
target_world_pos = dp.Vector3(world_link_pos[0], world_link_pos[1],
world_link_pos[2])
#target_local_pos = self.mb.get_world_transform(-1).apply_inverse(target_world_pos)
all_joint_angles2 = dp.inverse_kinematics(self.mb, link_id,
target_world_pos)
#print("--")
#print("len(all_joint_angles)=",len(all_joint_angles))
#print("all_joint_angles=",all_joint_angles)
vec = []
for i in range(all_joint_angles2.size() - 7):
vec.append(all_joint_angles2[i + 7])
#print("len(all_joint_angles2)=",len(vec))
#print("all_joint_angles2=",vec)
vec = np.array(vec)
#diff = vec - all_joint_angles
#print("diff=",diff)
# Extract the relevant joint angles.
joint_angles = [vec[i] for i in joint_ids]
return joint_angles