def rot_matrix_from_vecs(vec_a, vec_b):
out = Rotation()
vec_a.Normalize()
vec_b.Normalize()
vcross = vec_a * vec_b
vdot = dot(vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.1:
return out
# Or in the opposite direction
elif 1.0 + vdot < 0.1:
nx = Vector(1, 0, 0)
temp_dot = dot(vec_a, nx)
if -0.9 < abs(temp_dot) < 0.9:
axis = vec_a * nx
out = out.Rot(axis, 3.14)
else:
ny = Vector(0, 1, 0)
axis = vec_a * ny
out = out.Rot(axis, 3.14)
else:
skew_v = skew_mat(vcross)
out = add_mat(
add_mat(Rotation(), skew_v),
scalar_mul(skew_v * skew_v, (1 - vdot) / (vcross.Norm()**2)))
return out
def get_angle(vec_a, vec_b, up_vector=None):
vec_a.Normalize()
vec_b.Normalize()
cross_ab = vec_a * vec_b
vdot = dot(vec_a, vec_b)
# print('VDOT', vdot, vec_a, vec_b)
# Check if the vectors are in the same direction
if 1.0 - vdot < 0.000001:
angle = 0.0
# Or in the opposite direction
elif 1.0 + vdot < 0.000001:
angle = np.pi
else:
angle = math.acos(vdot)
if up_vector is not None:
same_dir = np.sign(dot(cross_ab, up_vector))
if same_dir < 0.0:
angle = -angle
return angle
def load_joint_data(self, ambf_config, urdf_joint):
joint_yaml_name = self.add_joint_prefix_str(urdf_joint.attrib['name'])
if urdf_joint.attrib['type'] in [
'revolute', 'continuous', 'prismatic', 'fixed'
]:
joint = JointTemplate()
joint_data = joint.ambf_data
parent_body = self._bodies_map[urdf_joint.find(
'parent').attrib['link']]
child_body = self._bodies_map[urdf_joint.find(
'child').attrib['link']]
joint_data['name'] = urdf_joint.attrib['name']
joint_data['type'] = urdf_joint.attrib['type']
joint_data['parent'] = self.add_body_prefix_str(
urdf_joint.find('parent').attrib['link'])
joint_data['child'] = self.add_body_prefix_str(
urdf_joint.find('child').attrib['link'])
joint.origin = to_kdl_frame(urdf_joint.find('origin'))
if urdf_joint.attrib['type'] == 'fixed':
# If the joint is fixed, urdfs joint axis is ignore, we set it to universal nz
joint.axis = Vector(0, 0, 1)
else:
joint.axis = to_kdl_vec(urdf_joint.find('axis'))
parent_temp_frame = parent_body.visual_offset.Inverse(
) * joint.origin
parent_pivot = parent_temp_frame.p
parent_axis = parent_temp_frame.M * joint.axis
parent_pivot_data = joint_data["parent pivot"]
parent_axis_data = joint_data["parent axis"]
assign_xyz(parent_pivot_data, parent_pivot)
assign_xyz(parent_axis_data, parent_axis)
child_temp_frame = child_body.visual_offset
inv_child_temp_frame = child_temp_frame.Inverse()
child_pivot = inv_child_temp_frame.p
child_axis = inv_child_temp_frame.M * joint.axis
# The use of pivot and axis does not fully define the connection and relative transform between two bodies
# it is very likely that we need an additional offset of the child body as in most of the cases of URDF's
# For this purpose, we calculate the offset as follows
r_c_p_ambf = rot_matrix_from_vecs(child_axis, parent_axis)
r_c_p_urdf = parent_body.visual_offset.M.Inverse(
) * joint.origin.M * child_body.visual_offset.M
r_angular_offset = r_c_p_ambf.Inverse() * r_c_p_urdf
offset_axis_angle = r_angular_offset.GetRotAngle()
# print(axis_angle[0]),
# print(round(axis_angle[1][0], 1), round(axis_angle[1][1], 1), round(axis_angle[1][2], 1))
if abs(offset_axis_angle[0]) > 0.01:
# print '*****************************'
# print joint_data['name']
# print 'Joint Axis, '
# print '\t', joint.axis
# print 'Offset Axis'
# print '\t', offset_axis_angle[1]
offset_angle = round(offset_axis_angle[0], 3)
offset_axis = offset_axis_angle[1]
# print 'Offset Angle: \t', offset_angle
if abs(1.0 - dot(child_axis, offset_axis_angle[1])) < 0.1:
joint_data['offset'] = offset_angle
# print ': SAME DIRECTION'
elif abs(1.0 + dot(child_axis, offset_axis_angle[1])) < 0.1:
joint_data['offset'] = -offset_angle
# print ': OPPOSITE DIRECTION'
else:
print('ERROR ', joint_data['name'], 'type',
urdf_joint.attrib['type'], ': SHOULD\'NT GET HERE')
# print ('Offset Angle: ', offset_angle)
# print ('Offset Axis: ', offset_axis)
# print ('Joint Axis: ', joint.axis)
# r_c_p_ambf = self.round_mat(r_c_p_ambf)
# r_c_p_urdf = self.round_mat(r_c_p_urdf)
# r_angular_offset = self.round_mat(r_angular_offset)
#
# print ('R URDF: ')
# print (r_c_p_urdf)
# print ('R AMBF')
# print (r_c_p_ambf)
# print ('R_DIFF')
# print (r_angular_offset)
# print ('-----------------------------')
child_pivot_data = joint_data["child pivot"]
child_axis_data = joint_data["child axis"]
# There is a bug in bullet discussed here:
# https: // github.com / bulletphysics / bullet3 / issues / 2031
# As a work around, we want to tweak the axis or body masses just a bit
# It's better to tweak masses than axes
if parent_body.ambf_data['mass'] > 0.0:
factA = 1.0 / parent_body.ambf_data['mass']
if child_body.ambf_data['mass'] > 0.0:
factB = 1.0 / child_body.ambf_data['mass']
weighted_axis = factA * parent_axis + factB * child_axis
if weighted_axis.Norm() < 0.001:
print(
"WARNING: ",
"Weighted Axis for joint \"%s\" is zero, to avoid breaking Bullet, "
"increasing the mass of parent body \"%s\" and decreasing the mass"
" of child body \"%s\" by 1%%" %
(joint.ambf_data['name'],
parent_body.ambf_data['name'],
child_body.ambf_data['name']))
parent_body.ambf_data[
'mass'] = parent_body.ambf_data['mass'] * 1.01
child_body.ambf_data[
'mass'] = child_body.ambf_data['mass'] * 0.99
assign_xyz(child_pivot_data, child_pivot)
assign_xyz(child_axis_data, child_axis)
if urdf_joint.attrib['type'] == 'continuous':
del joint_data['joint limits']['low']
del joint_data['joint limits']['high']
else:
urdf_joint_limit = urdf_joint.find('limit')
if urdf_joint_limit is not None:
joint_limit_data = joint_data["joint limits"]
joint_limit_data['low'] = round(
float(urdf_joint_limit.attrib['lower']), 3)
joint_limit_data['high'] = round(
float(urdf_joint_limit.attrib['upper']), 3)
ambf_config[joint_yaml_name] = joint_data
self._joint_names_list.append(joint_yaml_name)
def calculate_joint_angles(parent_quat, child_quat): UNIT_X = Vector(1,0,0) UNIT_Y = Vector(0,1,0) UNIT_Z = Vector(0,0,1) quat0 = Rotation.Quaternion(*parent_quat) quat1 = Rotation.Quaternion(*child_quat) forward0 = quat0 * UNIT_Z up0 = quat0 * UNIT_Y right0 = quat0 * UNIT_X forward1 = quat1 * UNIT_Z up1 = quat1 * UNIT_Y right1 = quat1 * UNIT_X ## Calculate the angle between the right vectors and the axis vector perpendicular to them angle = math.acos(max(min(dot(right1,right0), 1.0), -1.0)) axis = right1 * right0 axis.Normalize() ## Transform the forward vector of the child bone so that it is on the same horizontal plane as the forward vector of the parent bone transformed_forward1 = Rotation.Rot(axis, angle) * forward1 transformed_forward1.Normalize() ## Calculate the angle between the transformed forward vector and the forward vector of the parent bone ## This is the angle of the X-axis x_angle = math.acos(max(min(dot(transformed_forward1,forward0), 1.0), -1.0)) ## Calculate a vector perpendicular to the transformed forward vector and the forward vector of the parent bone axis = transformed_forward1 * forward0 axis.Normalize() ## Transform the forward vector of the child bone so that it is on the same vertical plane as the forward vector of the parent bone ## and to transform the up vector of the child bone to be used in a later calculation axis_ang = Rotation.Rot(axis, x_angle) transformed_forward1 = axis_ang * forward1 transformed_forward1.Normalize() transformed_up1 = axis_ang * up1 transformed_up1.Normalize() ## Set the sign of y_angle using the axis calculated and the right vector of the parent bone x_angle = math.copysign(x_angle, dot(axis, right0)) ## Calculate the angle between the transformed forward vector and the forward vector of the parent bone ## This is the angle of the Y-axis y_angle = math.acos(max(min(dot(transformed_forward1, forward0), 1.0), -1.0)) ## Calculate a vector perpendicular to the transformed forward vector and the forward vector of the parent bone axis = transformed_forward1 * forward0 axis.Normalize() ## Transform the transformed up vector so that it is on the same vertical plane as the up vector of the parent bone transformed_up1 = Rotation.Rot(axis, x_angle) * transformed_up1 transformed_up1.Normalize() ## Set the sign of x_angle using the axis calculated and the up vector of the parent bone y_angle = math.copysign(y_angle, dot(axis, up0)) ## Calculate the angle between the transformed up vector and the up vector of the parent bone ## This is the angle of the Z-axis z_angle = math.acos(max(min(dot(transformed_up1, up0), 1.0), -1.0)) axis = transformed_up1 * up0 axis.Normalize() ## Set the sign of z_angle using the axis calculated and the forward vector of the parent bone z_angle = math.copysign(z_angle, dot(axis, forward0)) return [x_angle, y_angle, z_angle]