def get_additional_states(self): """ :return: non-perception observation, such as goal location """ additional_states = self.global_to_local(self.target_pos)[:2] if self.goal_format == 'polar': additional_states = np.array( cartesian_to_polar(additional_states[0], additional_states[1])) # linear velocity along the x-axis linear_velocity = rotate_vector_3d( self.robots[0].get_linear_velocity(), *self.robots[0].get_rpy())[0] # angular velocity along the z-axis angular_velocity = rotate_vector_3d( self.robots[0].get_angular_velocity(), *self.robots[0].get_rpy())[2] additional_states = np.append(additional_states, [linear_velocity, angular_velocity]) if self.config['task'] == 'reaching': end_effector_pos_local = self.global_to_local( self.robots[0].get_end_effector_position()) additional_states = np.append(additional_states, end_effector_pos_local) assert additional_states.shape[0] == self.additional_states_dim, \ 'additional states dimension mismatch {} v.s. {}'.format(additional_states.shape[0], self.additional_states_dim) return additional_states
def global_to_local(self, pos): """ Convert a 3D point in global frame to agent's local frame :param pos: a 3D point in global frame :return: the same 3D point in agent's local frame """ return rotate_vector_3d(pos - self.robots[0].get_position(), *self.robots[0].get_rpy())
def get_additional_states(self): """ :return: non-perception observation, such as goal location """ additional_states = [] additional_states = self.global_to_local(self.target_pos)[:2] if self.goal_format == 'polar': additional_states = np.array(cartesian_to_polar(additional_states[0], additional_states[1])) if self.config['task'] == 'reaching': additional_states = np.append(additional_states, self.target_pos[2:]) #additional_states = [] # linear velocity along the x-axis linear_velocity = rotate_vector_3d(self.robots[0].get_linear_velocity(), *self.robots[0].get_rpy())[0] # angular velocity along the z-axis angular_velocity = rotate_vector_3d(self.robots[0].get_angular_velocity(), *self.robots[0].get_rpy())[2] additional_states = np.append(additional_states, [linear_velocity, angular_velocity]) if self.config['task'] == 'reaching': # End-effector end_effector_pos_local = self.global_to_local(self.robots[0].get_end_effector_position()) additional_states = np.append(additional_states, end_effector_pos_local) # Height #additional_states = np.append(additional_states, self.target_pos[2:]) # L2 distance between end-effector and goal #additional_states = np.append(additional_states, self.get_l2_potential()) # Joint positions and velocities self.robots[0].calc_state() additional_states = np.append(additional_states, np.sin(self.robots[0].joint_position[2:])) additional_states = np.append(additional_states, np.cos(self.robots[0].joint_position[2:])) additional_states = np.append(additional_states, self.robots[0].joint_velocity[2:]) #additional_states = np.append(additional_states, self.robots[0].joint_torque) assert additional_states.shape[0] == self.additional_states_dim, \ 'additional states dimension mismatch {} v.s. {}'.format(additional_states.shape[0], self.additional_states_dim) return additional_states
def get_additional_states(self): relative_position = self.target_pos - self.robots[0].get_position() # rotate relative position back to body point of view additional_states = rotate_vector_3d(relative_position, *self.robots[0].get_rpy()) if self.config['task'] == 'reaching': end_effector_pos = self.robots[0].get_end_effector_position( ) - self.robots[0].get_position() end_effector_pos = rotate_vector_3d(end_effector_pos, *self.robots[0].get_rpy()) additional_states = np.concatenate( (additional_states, end_effector_pos)) assert len( additional_states ) == self.additional_states_dim, 'additional states dimension mismatch' return additional_states """
def calc_state(self): j = np.array([ j.get_joint_relative_state() for j in self.ordered_joints ]).astype(np.float32).flatten() self.joint_position = j[0::3] self.joint_velocity = j[1::3] self.joint_torque = j[2::3] self.joint_at_limit = np.count_nonzero( np.abs(self.joint_position) > 0.99) pos = self.get_position() rpy = self.get_rpy() # rotate linear and angular velocities to local frame lin_vel = rotate_vector_3d(self.get_linear_velocity(), *rpy) ang_vel = rotate_vector_3d(self.get_angular_velocity(), *rpy) state = np.concatenate([pos, rpy, lin_vel, ang_vel, j]) return state
def get_task_obs(self, env): """ Get task-specific observation, including goal position, current velocities, etc. :param env: environment instance :return: task-specific observation """ task_obs = self.global_to_local(env, self.target_pos)[:2] if self.goal_format == 'polar': task_obs = np.array(cartesian_to_polar(task_obs[0], task_obs[1])) # linear velocity along the x-axis linear_velocity = rotate_vector_3d(env.robots[0].get_linear_velocity(), *env.robots[0].get_rpy())[0] # angular velocity along the z-axis angular_velocity = rotate_vector_3d( env.robots[0].get_angular_velocity(), *env.robots[0].get_rpy())[2] task_obs = np.append(task_obs, [linear_velocity, angular_velocity]) return task_obs
def calc_state(self): j = np.array( [j.get_joint_relative_state() for j in self.ordered_joints], dtype=np.float32).flatten() self.joint_speeds = j[1::3] self.joint_torque = j[2::3] self.joints_at_limit = np.count_nonzero(np.abs(j[0::3]) > 0.99) z = self.robot_body.get_position()[2] r, p, yaw = self.robot_body.get_rpy() # rotate speed back to body point of view vx, vy, vz = rotate_vector_3d(self.robot_body.velocity(), r, p, yaw) more = np.array([z, vx, vy, vz, r, p, yaw], dtype=np.float32) return np.clip(np.concatenate([more] + [j]), -5, +5)
def save_scaled_urdf(filename, avg_size_mass, obj_class): model_path = os.path.dirname(filename) meta_json = os.path.join(model_path, 'misc/metadata.json') if os.path.isfile(meta_json): with open(meta_json, 'r') as f: meta_data = json.load(f) bbox_size = np.array(meta_data['bbox_size']) base_link_offset = np.array(meta_data['base_link_offset']) else: bbox_json = os.path.join(model_path, 'misc/bbox.json') with open(bbox_json, 'r') as bbox_file: bbox_data = json.load(bbox_file) bbox_max = np.array(bbox_data['max']) bbox_min = np.array(bbox_data['min']) bbox_size = bbox_max - bbox_min base_link_offset = (bbox_min + bbox_max) / 2.0 bounding_box = np.array(avg_size_mass['size']) scale = bounding_box / bbox_size # scale = np.array([1.0, 1.0, 1.0]) object_tree = ET.parse(filename) # We need to scale 1) the meshes, 2) the position of meshes, 3) the position of joints, 4) the orientation axis of joints # The problem is that those quantities are given wrt. its parent link frame, and this can be rotated wrt. the frame the scale was given in # Solution: parse the kin tree joint by joint, extract the rotation, rotate the scale, apply rotated scale to 1, 2, 3, 4 in the child link frame # First, define the scale in each link reference frame # and apply it to the joint values scales_in_lf = {} scales_in_lf["base_link"] = scale all_processed = False while not all_processed: all_processed = True for joint in object_tree.iter("joint"): parent_link_name = joint.find("parent").attrib["link"] child_link_name = joint.find("child").attrib["link"] if parent_link_name in scales_in_lf and child_link_name not in scales_in_lf: scale_in_parent_lf = scales_in_lf[parent_link_name] # The location of the joint frame is scaled using the scale in the parent frame for origin in joint.iter("origin"): current_origin_xyz = np.array([ float(val) for val in origin.attrib["xyz"].split(" ") ]) new_origin_xyz = np.multiply(current_origin_xyz, scale_in_parent_lf) new_origin_xyz = np.array( [round_up(val, 4) for val in new_origin_xyz]) origin.attrib['xyz'] = ' '.join(map(str, new_origin_xyz)) # scale the prismatic joint if joint.attrib['type'] == 'prismatic': limits = joint.findall('limit') assert len(limits) == 1 limit = limits[0] axes = joint.findall('axis') assert len(axes) == 1 axis = axes[0] axis_np = np.array( [float(elem) for elem in axis.attrib['xyz'].split()]) major_axis = np.argmax(np.abs(axis_np)) # assume the prismatic joint is roughly axis-aligned limit.attrib['upper'] = str( float(limit.attrib['upper']) * scale_in_parent_lf[major_axis]) limit.attrib['lower'] = str( float(limit.attrib['lower']) * scale_in_parent_lf[major_axis]) # Get the rotation of the joint frame and apply it to the scale if "rpy" in joint.keys(): joint_frame_rot = np.array( [float(val) for val in joint.attrib['rpy'].split(" ")]) # Rotate the scale scale_in_child_lf = rotate_vector_3d(scale_in_parent_lf, *joint_frame_rot, cck=True) scale_in_child_lf = np.absolute(scale_in_child_lf) else: scale_in_child_lf = scale_in_parent_lf # print("Adding: ", joint.find("child").attrib["link"]) scales_in_lf[joint.find( "child").attrib["link"]] = scale_in_child_lf # The axis of the joint is defined in the joint frame, we scale it after applying the rotation for axis in joint.iter("axis"): current_axis_xyz = np.array( [float(val) for val in axis.attrib["xyz"].split(" ")]) new_axis_xyz = np.multiply(current_axis_xyz, scale_in_child_lf) new_axis_xyz /= np.linalg.norm(new_axis_xyz) new_axis_xyz = np.array( [round_up(val, 4) for val in new_axis_xyz]) axis.attrib['xyz'] = ' '.join(map(str, new_axis_xyz)) # Iterate again the for loop since we added new elements to the dictionary all_processed = False all_links = object_tree.findall('link') all_links_trimesh = [] total_volume = 0.0 for link in all_links: meshes = link.findall('collision/geometry/mesh') if len(meshes) == 0: all_links_trimesh.append(None) continue assert len(meshes) == 1, (filename, link.attrib['name']) collision_mesh_path = os.path.join(model_path, meshes[0].attrib['filename']) trimesh_obj = trimesh.load(file_obj=collision_mesh_path) all_links_trimesh.append(trimesh_obj) volume = trimesh_obj.volume if link.attrib['name'] == 'base_link': if obj_class in ['lamp']: volume *= 10.0 total_volume += volume # Scale the mass based on bounding box size # TODO: how to scale moment of inertia? total_mass = avg_size_mass['density'] * \ bounding_box[0] * bounding_box[1] * bounding_box[2] print('total_mass', total_mass) density = total_mass / total_volume print('avg density', density) for trimesh_obj in all_links_trimesh: if trimesh_obj is not None: trimesh_obj.density = density assert len(all_links) == len(all_links_trimesh) # Now iterate over all links and scale the meshes and positions for link, link_trimesh in zip(all_links, all_links_trimesh): inertials = link.findall('inertial') if len(inertials) == 0: inertial = ET.SubElement(link, 'inertial') else: assert len(inertials) == 1 inertial = inertials[0] masses = inertial.findall('mass') if len(masses) == 0: mass = ET.SubElement(inertial, 'mass') else: assert len(masses) == 1 mass = masses[0] inertias = inertial.findall('inertia') if len(inertias) == 0: inertia = ET.SubElement(inertial, 'inertia') else: assert len(inertias) == 1 inertia = inertias[0] origins = inertial.findall('origin') if len(origins) == 0: origin = ET.SubElement(inertial, 'origin') else: assert len(origins) == 1 origin = origins[0] if link_trimesh is not None: if link.attrib['name'] == 'base_link': if obj_class in ['lamp']: link_trimesh.density *= 10.0 if link_trimesh.is_watertight: center = link_trimesh.center_mass else: center = link_trimesh.centroid # The inertial frame origin will be scaled down below. # Here, it has the value BEFORE scaling origin.attrib['xyz'] = ' '.join(map(str, center)) origin.attrib['rpy'] = ' '.join(map(str, [0.0, 0.0, 0.0])) mass.attrib['value'] = str(round_up(link_trimesh.mass, 4)) moment_of_inertia = link_trimesh.moment_inertia inertia.attrib['ixx'] = str(moment_of_inertia[0][0]) inertia.attrib['ixy'] = str(moment_of_inertia[0][1]) inertia.attrib['ixz'] = str(moment_of_inertia[0][2]) inertia.attrib['iyy'] = str(moment_of_inertia[1][1]) inertia.attrib['iyz'] = str(moment_of_inertia[1][2]) inertia.attrib['izz'] = str(moment_of_inertia[2][2]) else: # empty link that does not have any mesh origin.attrib['xyz'] = ' '.join(map(str, [0.0, 0.0, 0.0])) origin.attrib['rpy'] = ' '.join(map(str, [0.0, 0.0, 0.0])) mass.attrib['value'] = str(0.0) inertia.attrib['ixx'] = str(0.0) inertia.attrib['ixy'] = str(0.0) inertia.attrib['ixz'] = str(0.0) inertia.attrib['iyy'] = str(0.0) inertia.attrib['iyz'] = str(0.0) inertia.attrib['izz'] = str(0.0) scale_in_lf = scales_in_lf[link.attrib["name"]] # Apply the scale to all mesh elements within the link (original scale and origin) for mesh in link.iter("mesh"): if "scale" in mesh.attrib: mesh_scale = np.array( [float(val) for val in mesh.attrib["scale"].split(" ")]) new_scale = np.multiply(mesh_scale, scale_in_lf) new_scale = np.array([round_up(val, 4) for val in new_scale]) mesh.attrib['scale'] = ' '.join(map(str, new_scale)) else: new_scale = np.array([round_up(val, 4) for val in scale_in_lf]) mesh.set('scale', ' '.join(map(str, new_scale))) for origin in link.iter("origin"): origin_xyz = np.array( [float(val) for val in origin.attrib["xyz"].split(" ")]) new_origin_xyz = np.multiply(origin_xyz, scale_in_lf) new_origin_xyz = np.array( [round_up(val, 4) for val in new_origin_xyz]) origin.attrib['xyz'] = ' '.join(map(str, new_origin_xyz)) new_filename = filename[:-5] + '_avg_size' urdfs_no_floating = save_urdfs_without_floating_joints( object_tree, new_filename, False) # If the object is broken down to multiple URDF files, we only want to # visulize the main URDF (e.g. visusalize the bed and ignore the pillows) # The main URDF is the one with the highest mass. max_mass = 0.0 main_urdf_file = None for key in urdfs_no_floating: object_tree = ET.parse(urdfs_no_floating[key][0]) cur_mass = 0.0 for mass in object_tree.iter('mass'): cur_mass += float(mass.attrib['value']) if cur_mass > max_mass: max_mass = cur_mass main_urdf_file = urdfs_no_floating[key][0] assert main_urdf_file is not None # Finally, we need to know where is the base_link origin wrt. the bounding box center. That allows us to place the model # correctly since the joint transformations given in the scene urdf are for the bounding box center # Coordinates of the bounding box center in the base_link frame # We scale the location. We will subtract this to the joint location scaled_bbxc_in_blf = -scale * base_link_offset return main_urdf_file, scaled_bbxc_in_blf
def scale_object(self): """ Scale the object according to the given bounding box """ # We need to scale 1) the meshes, 2) the position of meshes, 3) the position of joints, 4) the orientation # axis of joints. The problem is that those quantities are given wrt. its parent link frame, and this can be # rotated wrt. the frame the scale was given in Solution: parse the kin tree joint by joint, extract the # rotation, rotate the scale, apply rotated scale to 1, 2, 3, 4 in the child link frame # First, define the scale in each link reference frame # and apply it to the joint values scales_in_lf = {"base_link": self.scale} all_processed = False while not all_processed: all_processed = True for joint in self.object_tree.iter("joint"): parent_link_name = joint.find("parent").attrib["link"] child_link_name = joint.find("child").attrib["link"] if parent_link_name in scales_in_lf and child_link_name not in scales_in_lf: scale_in_parent_lf = scales_in_lf[parent_link_name] # The location of the joint frame is scaled using the scale in the parent frame for origin in joint.iter("origin"): current_origin_xyz = np.array( [float(val) for val in origin.attrib["xyz"].split(" ")]) new_origin_xyz = np.multiply( current_origin_xyz, scale_in_parent_lf) new_origin_xyz = np.array( [round_up(val, 10) for val in new_origin_xyz]) origin.attrib['xyz'] = ' '.join( map(str, new_origin_xyz)) # scale the prismatic joint if joint.attrib['type'] == 'prismatic': limits = joint.findall('limit') assert len(limits) == 1 limit = limits[0] axes = joint.findall('axis') assert len(axes) == 1 axis = axes[0] axis_np = np.array([ float(elem) for elem in axis.attrib['xyz'].split()]) major_axis = np.argmax(np.abs(axis_np)) # assume the prismatic joint is roughly axis-aligned limit.attrib['upper'] = str( float(limit.attrib['upper']) * scale_in_parent_lf[major_axis]) limit.attrib['lower'] = str( float(limit.attrib['lower']) * scale_in_parent_lf[major_axis]) # Get the rotation of the joint frame and apply it to the scale if "rpy" in joint.keys(): joint_frame_rot = np.array( [float(val) for val in joint.attrib['rpy'].split(" ")]) # Rotate the scale scale_in_child_lf = rotate_vector_3d( scale_in_parent_lf, *joint_frame_rot, cck=True) scale_in_child_lf = np.absolute(scale_in_child_lf) else: scale_in_child_lf = scale_in_parent_lf # print("Adding: ", joint.find("child").attrib["link"]) scales_in_lf[joint.find("child").attrib["link"]] = \ scale_in_child_lf # The axis of the joint is defined in the joint frame, we scale it after applying the rotation for axis in joint.iter("axis"): current_axis_xyz = np.array( [float(val) for val in axis.attrib["xyz"].split(" ")]) new_axis_xyz = np.multiply( current_axis_xyz, scale_in_child_lf) new_axis_xyz /= np.linalg.norm(new_axis_xyz) new_axis_xyz = np.array( [round_up(val, 10) for val in new_axis_xyz]) axis.attrib['xyz'] = ' '.join(map(str, new_axis_xyz)) # Iterate again the for loop since we added new elements to the dictionary all_processed = False all_links = self.object_tree.findall('link') # compute dynamics properties if self.category not in ["walls", "floors", "ceilings"]: all_links_trimesh = [] total_volume = 0.0 for link in all_links: meshes = link.findall('collision/geometry/mesh') if len(meshes) == 0: all_links_trimesh.append(None) continue # assume one collision mesh per link assert len(meshes) == 1, (self.filename, link.attrib['name']) collision_mesh_path = os.path.join(self.model_path, meshes[0].attrib['filename']) trimesh_obj = trimesh.load( file_obj=collision_mesh_path, force='mesh') all_links_trimesh.append(trimesh_obj) volume = trimesh_obj.volume # a hack to artificially increase the density of the lamp base if link.attrib['name'] == 'base_link': if self.category in ['lamp']: volume *= 10.0 total_volume += volume # avg L x W x H and Weight is given for this object category if self.avg_obj_dims is not None: avg_density = self.avg_obj_dims['density'] # otherwise, use the median density across all existing object categories else: avg_density = 67.0 # Scale the mass based on bounding box size # TODO: how to scale moment of inertia? total_mass = avg_density * \ self.bounding_box[0] * \ self.bounding_box[1] * self.bounding_box[2] # print('total_mass', total_mass) density = total_mass / total_volume # print('avg density', density) for trimesh_obj in all_links_trimesh: if trimesh_obj is not None: trimesh_obj.density = density assert len(all_links_trimesh) == len(all_links) # Now iterate over all links and scale the meshes and positions for i, link in enumerate(all_links): if self.category not in ["walls", "floors", "ceilings"]: link_trimesh = all_links_trimesh[i] # assign dynamics properties inertials = link.findall('inertial') if len(inertials) == 0: inertial = ET.SubElement(link, 'inertial') else: assert len(inertials) == 1 inertial = inertials[0] masses = inertial.findall('mass') if len(masses) == 0: mass = ET.SubElement(inertial, 'mass') else: assert len(masses) == 1 mass = masses[0] inertias = inertial.findall('inertia') if len(inertias) == 0: inertia = ET.SubElement(inertial, 'inertia') else: assert len(inertias) == 1 inertia = inertias[0] origins = inertial.findall('origin') if len(origins) == 0: origin = ET.SubElement(inertial, 'origin') else: assert len(origins) == 1 origin = origins[0] if link_trimesh is not None: # a hack to artificially increase the density of the lamp base if link.attrib['name'] == 'base_link': if self.category in ['lamp']: link_trimesh.density *= 10.0 if link_trimesh.is_watertight: center = link_trimesh.center_mass else: center = link_trimesh.centroid # The inertial frame origin will be scaled down below. # Here, it has the value BEFORE scaling origin.attrib['xyz'] = ' '.join(map(str, center)) origin.attrib['rpy'] = ' '.join(map(str, [0.0, 0.0, 0.0])) mass.attrib['value'] = str(round_up(link_trimesh.mass, 10)) moment_of_inertia = link_trimesh.moment_inertia inertia.attrib['ixx'] = str(moment_of_inertia[0][0]) inertia.attrib['ixy'] = str(moment_of_inertia[0][1]) inertia.attrib['ixz'] = str(moment_of_inertia[0][2]) inertia.attrib['iyy'] = str(moment_of_inertia[1][1]) inertia.attrib['iyz'] = str(moment_of_inertia[1][2]) inertia.attrib['izz'] = str(moment_of_inertia[2][2]) else: # empty link that does not have any mesh origin.attrib['xyz'] = ' '.join(map(str, [0.0, 0.0, 0.0])) origin.attrib['rpy'] = ' '.join(map(str, [0.0, 0.0, 0.0])) mass.attrib['value'] = str(0.0) inertia.attrib['ixx'] = str(0.0) inertia.attrib['ixy'] = str(0.0) inertia.attrib['ixz'] = str(0.0) inertia.attrib['iyy'] = str(0.0) inertia.attrib['iyz'] = str(0.0) inertia.attrib['izz'] = str(0.0) scale_in_lf = scales_in_lf[link.attrib["name"]] # Apply the scale to all mesh elements within the link (original scale and origin) for mesh in link.iter("mesh"): if "scale" in mesh.attrib: mesh_scale = np.array( [float(val) for val in mesh.attrib["scale"].split(" ")]) new_scale = np.multiply(mesh_scale, scale_in_lf) new_scale = np.array([round_up(val, 10) for val in new_scale]) mesh.attrib['scale'] = ' '.join(map(str, new_scale)) else: new_scale = np.array([round_up(val, 10) for val in scale_in_lf]) mesh.set('scale', ' '.join(map(str, new_scale))) for origin in link.iter("origin"): origin_xyz = np.array( [float(val) for val in origin.attrib["xyz"].split(" ")]) new_origin_xyz = np.multiply(origin_xyz, scale_in_lf) new_origin_xyz = np.array( [round_up(val, 10) for val in new_origin_xyz]) origin.attrib['xyz'] = ' '.join(map(str, new_origin_xyz))