ManiSkill
ManiSkill copied to clipboard
haosulab
The issue about incorrect object's center of mass.
Hello, I found that there might be some issues with obtaining the object's center of mass position.
The code for importing objects into the environment is as follows:
bowl = self.scene.create_actor_builder()
bowl.add_multiple_convex_collisions_from_file(
decomposition = "coacd",
filename = "/home/yzy/nus/database/shapenet/ShapeNetCore.v2/02992529/3a6a3db4a0174fddd2789f496481c83e/models/model_normalized.obj",
scale=[0.12, 0.12, 0.12]
)
Then, I used env.unwrapped.obj.pose.raw_pose & env.unwrapped.obj.cmass_local_pose.raw_pose to get the global position of object's center of mass. Meanwhile, I used env.unwrapped.obj.get_collision_meshes()[0] & mesh.center_mass to get the mesh's cmass.
I visualized mesh's cmass and global position of object's center of mass in a picture. The result is as follows (The green point is global position of object's center of mass and the red point is mesh's cmass. The gray points are the boundary vertices of the mesh.):
It can be seen that these two points do not overlap, and the red point should be more accurate. I cannot figure out the reason because the object's mass is uniform.
Note: There are two .py files and the complete code are as follows. The first file is script.py.
import argparse
import gymnasium as gym
import numpy as np
import env_creation_test
from mani_skill.envs.sapien_env import BaseEnv
from mani_skill.sensors.camera import CameraConfig
import trimesh
import trimesh.scene
import open3d as o3d
def parse_args(args=None):
parser = argparse.ArgumentParser()
parser.add_argument("-e", "--env-id", type=str, default="PushCup-yzy-test", help="The environment ID of the task you want to simulate")
parser.add_argument("--cam-width", type=int, help="Override the width of every camera in the environment")
parser.add_argument("--cam-height", type=int, help="Override the height of every camera in the environment")
parser.add_argument(
"-s",
"--seed",
type=int,
help="Seed the random actions and environment. Default is no seed",
)
args = parser.parse_args()
return args
#Compute the position of object
def compute_cmass_global_pose(cmass_local_pose,object_pose):
#Trans the object_pose.q to matrix
qx, qy, qz, qw = object_pose[0][-4:]
rotation_matrix = np.array([
[1 - 2*(qy**2 + qz**2), 2*(qx*qy - qz*qw), 2*(qx*qz + qy*qw)],
[2*(qx*qy + qz*qw), 1 - 2*(qx**2 + qz**2), 2*(qy*qz - qx*qw)],
[2*(qx*qz - qy*qw), 2*(qy*qz + qx*qw), 1 - 2*(qx**2 + qy**2)]
])
local_position = np.array(cmass_local_pose[:3])
world_position = np.dot(rotation_matrix, local_position) + np.array(object_pose[0][0:3])
return world_position
def main(args):
if args.seed is not None:
np.random.seed(args.seed)
sensor_configs = dict()
if args.cam_width:
sensor_configs["width"] = args.cam_width
if args.cam_height:
sensor_configs["height"] = args.cam_height
env: BaseEnv = gym.make(
args.env_id,
obs_mode="pointcloud",
reward_mode="none",
sensor_configs=sensor_configs,
)
obs, _ = env.reset(seed=args.seed)
while True:
action = env.action_space.sample()
obs, reward, terminated, truncated, info = env.step(action)
mesh = env.unwrapped.obj.get_collision_meshes()[0]
#Get the gravity center of the mesh
mass_center = mesh.center_mass.reshape(1,3)
#Set it as pointcloud
mesh_center_pcd = o3d.geometry.PointCloud()
mesh_center_pcd.points = o3d.utility.Vector3dVector(mass_center)
#Set its color
mesh_center_pcd.paint_uniform_color([1.0, 0.0, 0.0])
#Get the global position of the mass center
object_pose = env.unwrapped.obj.pose.raw_pose
cmass_pose_local = env.unwrapped.obj.cmass_local_pose.raw_pose[0]
#Get the global position
cmass_global_pos = compute_cmass_global_pose(cmass_pose_local,object_pose)
#Set it as pointcloud
obj_center_pcd = o3d.geometry.PointCloud()
obj_center_pcd.points = o3d.utility.Vector3dVector(cmass_global_pos.reshape(1,3))
#Set its color as green
obj_center_pcd.paint_uniform_color([0.0, 1.0, 0.0])
#Vis
pcd = o3d.geometry.PointCloud()
vertices = mesh.vertices
pcd.points = o3d.utility.Vector3dVector(vertices)
#Set color
pcd.paint_uniform_color([0.8, 0.8, 0.8])
o3d.visualization.draw_geometries([pcd,mesh_center_pcd,obj_center_pcd])
if terminated or truncated:
break
env.close()
if __name__ == "__main__":
main(parse_args())
The second file is env_creation_test.py.
from typing import Any, Dict, Union
import numpy as np
import sapien
import torch
import torch.random
import math
import mani_skill.agents.controllers.utils.kinematics as kinematics
from transforms3d.euler import euler2quat, quat2euler
from mani_skill.agents.robots import Fetch, Panda
from mani_skill.envs.sapien_env import BaseEnv
from mani_skill.sensors.camera import CameraConfig
from mani_skill.utils import common, sapien_utils
from mani_skill.utils.building import actors
from mani_skill.utils.registration import register_env
from mani_skill.utils.scene_builder.table import TableSceneBuilder
from mani_skill.utils.structs import Pose
from mani_skill.utils.structs.types import Array, GPUMemoryConfig, SimConfig
from mani_skill import PACKAGE_ASSET_DIR
@register_env("PushCup-yzy-test", max_episode_steps=50)
class PushCupEnv(BaseEnv):
_sample_video_link = "https://github.com/haosulab/ManiSkill/raw/main/figures/environment_demos/PushCube-v1_rt.mp4"
SUPPORTED_ROBOTS = ["panda", "fetch"]
# Specify some supported robot types
agent: Union[Panda, Fetch]
goal_radius = 0.01
re_orientation = True
def __init__(self, *args, robot_uids="panda", robot_init_qpos_noise=0.02, **kwargs):
# specifying robot_uids="panda" as the default means gym.make("PushCube-yzy") will default to using the panda arm.
self.robot_init_qpos_noise = robot_init_qpos_noise
self.goal_q = torch.tensor([0,0,math.pi/2])
super().__init__(*args, robot_uids=robot_uids, **kwargs)
# Specify default simulation/gpu memory configurations to override any default values
@property
def _default_sim_config(self):
return SimConfig(
gpu_memory_config=GPUMemoryConfig(
found_lost_pairs_capacity=2**25, max_rigid_patch_count=2**18
)
)
@property
def _default_sensor_configs(self):
# registers one 128x128 camera looking at the robot, cube, and target
# a smaller sized camera will be lower quality, but render faster
pose = sapien_utils.look_at(eye=[-0.1, 0, 0.4], target=[-0.1 + 0.0001, 0, 0])
return [
CameraConfig(
"base_camera",
pose=pose,
width=128,
height=106,
fov=np.pi / 2,
near=0.01,
far=100,
)
]
@property
def _default_human_render_camera_configs(self):
# registers a more high-definition (512x512) camera used just for rendering when render_mode="rgb_array" or calling env.render_rgb_array()
pose = sapien_utils.look_at([0.6, 0.7, 0.6], [0.0, 0.0, 0.35])
return CameraConfig(
"render_camera", pose=pose, width=512, height=512, fov=1, near=0.01, far=100
)
def _load_agent(self, options: dict):
# set a reasonable initial pose for the agent that doesn't intersect other objects
super()._load_agent(options, sapien.Pose(p=[-0.615, 0, 0]))
def _load_scene(self, options: dict):
# we use a prebuilt scene builder class that automatically loads in a floor and table.
self.table_scene = TableSceneBuilder(
env=self, robot_init_qpos_noise=self.robot_init_qpos_noise
)
self.table_scene.build()
bowl = self.scene.create_actor_builder()
bowl.add_multiple_convex_collisions_from_file(
decomposition = "coacd",
filename = "/home/yzy/nus/database/shapenet/ShapeNetCore.v2/02992529/3a6a3db4a0174fddd2789f496481c83e/models/model_normalized.obj",
scale=[0.12, 0.12, 0.12]
)
bowl.add_visual_from_file(
filename = "/home/yzy/nus/database/shapenet/ShapeNetCore.v2/02992529/3a6a3db4a0174fddd2789f496481c83e/models/model_normalized.obj",
scale=[0.12, 0.12, 0.12]
)
bowl.initial_pose = sapien.Pose(p=[0, 0, 0])
self.obj = bowl.build(name="bowl_yzy")
# we also add in red/white target to visualize where we want the cube to be pushed to
# we specify add_collisions=False as we only use this as a visual for videos and do not want it to affect the actual physics
# we finally specify the body_type to be "kinematic" so that the object stays in place
self.goal_region = actors.build_red_white_target(
self.scene,
radius=self.goal_radius,
thickness=1e-5,
name="goal_region",
add_collision=False,
body_type="kinematic",
initial_pose=sapien.Pose(p=[0, 0, 1e-3]),
)
def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
# use the torch.device context manager to automatically create tensors on CPU or CUDA depending on self.device, the device the environment runs on
with torch.device(self.device):
b = len(env_idx)
self.table_scene.initialize(env_idx)
nxyz = torch.zeros((b, 3))
nxyz = nxyz + torch.tensor([-0.1,0,0])
q = torch.tensor(euler2quat(0, np.pi, 0))
nxyz[..., 2] = 0.001
obj_pose = Pose.create_from_pq(p = nxyz, q=q)
self.obj.set_pose(obj_pose)
delta = torch.tensor([0.1, 0.2, 0])
target_region_xyz = nxyz + delta
traget_q = euler2quat(0, np.pi / 2, 0)
self.goal_region.set_pose(
Pose.create_from_pq(
p= target_region_xyz,
q= traget_q,
)
)
panda_kinematic = kinematics.Kinematics(urdf_path = f"{PACKAGE_ASSET_DIR}/robots/panda/panda_v2.urdf",
end_link_name = "panda_hand_tcp",
articulation = self.agent.robot,
active_joint_indices = torch.tensor([0,1,2,3,4,5,6])
)
arm_target_qpos = panda_kinematic.compute_ik(
target_pose= Pose.create_from_pq(p=np.array([0.4,0,0.4]), q=[0,-1,-0.1,0]),
q0=self.agent.robot.get_qpos(),
)
gipper_qpose = torch.zeros(b, 2) - 1
new_tensor = torch.cat((arm_target_qpos, gipper_qpose),dim=1)
self.agent.robot.set_qpos(new_tensor)
def evaluate(self):
is_obj_placed = (
torch.linalg.norm(
self.obj.pose.p[..., :2] - self.goal_region.pose.p[..., :2], axis=1
)
< self.goal_radius
) & (self.obj.pose.p[..., 2] < 1.05 + 5e-3)
if self.re_orientation == True:
euler_object = quat2euler(self.obj.pose.q[0])[2]
if euler_object < 0:
euler_object = math.pi*2 + euler_object
threshold = math.pi / 360
theta_diff = torch.abs(euler_object - self.goal_q[2])
is_equal = min(theta_diff, 2 * np.pi - theta_diff) < threshold
is_obj_placed = is_obj_placed & is_equal
return {"success": is_obj_placed}
def _get_obs_extra(self, info: Dict):
# some useful observation info for solving the task includes the pose of the tcp (tool center point) which is the point between the
# grippers of the robot
obs = dict(
tcp_pose=self.agent.tcp.pose.raw_pose,
)
if self.obs_mode_struct.use_state:
# if the observation mode requests to use state, we provide ground truth information about where the cube is.
# for visual observation modes one should rely on the sensed visual data to determine where the cube is
obs.update(
goal_pos=self.goal_region.pose.p,
obj_pose=self.obj.pose.raw_pose,
)
return obs
def compute_dense_reward(self, obs: Any, action: Array, info: Dict):
# We also create a pose marking where the robot should push the cube from that is easiest (pushing from behind the cube)
reward = 0
return reward
def compute_normalized_dense_reward(self, obs: Any, action: Array, info: Dict):
# this should be equal to compute_dense_reward / max possible reward
max_reward = 3.0
return self.compute_dense_reward(obs=obs, action=action, info=info) / max_reward
The last is the object. model_normalized.zip
