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))
