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]