sim control
This commit is contained in:
parent
2fcc650eb7
commit
1a0e3c8042
@ -5,5 +5,7 @@ from runners.simulator import Simulator
|
||||
class SimulateApp:
|
||||
@staticmethod
|
||||
def start():
|
||||
Simulator("configs/server/server_split_dataset_config.yaml").run()
|
||||
simulator = Simulator("configs/local/simulation_config.yaml")
|
||||
simulator.run("create")
|
||||
simulator.run("simulate")
|
||||
|
||||
@ -1,4 +1,3 @@
|
||||
|
||||
runner:
|
||||
general:
|
||||
seed: 0
|
||||
@ -11,4 +10,27 @@ runner:
|
||||
|
||||
simulation:
|
||||
robot:
|
||||
displaytable:
|
||||
urdf_path: "assets/franka_panda/panda.urdf"
|
||||
initial_position: [0, 0, 0] # 机械臂基座位置
|
||||
initial_orientation: [0, 0, 0] # 机械臂基座朝向(欧拉角)
|
||||
|
||||
turntable:
|
||||
radius: 0.3 # 转盘半径(米)
|
||||
height: 0.1 # 转盘高度
|
||||
center_position: [0.8, 0, 0.4]
|
||||
|
||||
target:
|
||||
obj_dir: /media/hofee/data/project/python/nbv_reconstruction/nbv_reconstruction/assets/object_meshes
|
||||
obj_name: "google_scan-box_0185"
|
||||
scale: 1.0 # 缩放系数
|
||||
mass: 0.1 # 质量(kg)
|
||||
rgba_color: [0.8, 0.8, 0.8, 1.0] # 目标物体颜色
|
||||
|
||||
camera:
|
||||
width: 640
|
||||
height: 480
|
||||
fov: 40
|
||||
near: 0.01
|
||||
far: 5.0
|
||||
|
||||
displaytable:
|
||||
@ -17,7 +17,7 @@ runner:
|
||||
plane_size: 10
|
||||
max_views: 512
|
||||
min_views: 128
|
||||
random_view_ratio: 0.02
|
||||
random_view_ratio: 0.002
|
||||
min_cam_table_included_degree: 20
|
||||
max_diag: 0.7
|
||||
min_diag: 0.01
|
||||
|
||||
@ -1,23 +1,456 @@
|
||||
import pybullet as p
|
||||
import pybullet_data
|
||||
import numpy as np
|
||||
import os
|
||||
import time
|
||||
from PytorchBoot.runners.runner import Runner
|
||||
import PytorchBoot.stereotype as stereotype
|
||||
from PytorchBoot.config import ConfigManager
|
||||
from utils.control import ControlUtil
|
||||
|
||||
|
||||
@stereotype.runner("simulator")
|
||||
class Simulator(Runner):
|
||||
CREATE: str = "create"
|
||||
SIMULATE: str = "simulate"
|
||||
INIT_GRIPPER_POSE:np.ndarray = np.asarray(
|
||||
[[0.41869126 ,0.87596275 , 0.23951774 , 0.36005292],
|
||||
[ 0.70787907 ,-0.4800251 , 0.51813998 ,-0.40499909],
|
||||
[ 0.56884584, -0.04739109 ,-0.82107382 ,0.76881103],
|
||||
[ 0. , 0. , 0. , 1. ]])
|
||||
TURNTABLE_WORLD_TO_PYBULLET_WORLD:np.ndarray = np.asarray(
|
||||
[[1, 0, 0, 0.8],
|
||||
[0, 1, 0, 0],
|
||||
[0, 0, 1, 0.5],
|
||||
[0, 0, 0, 1]])
|
||||
|
||||
debug_pose = np.asarray([
|
||||
[
|
||||
0.992167055606842,
|
||||
-0.10552699863910675,
|
||||
0.06684812903404236,
|
||||
-0.07388903945684433
|
||||
],
|
||||
[
|
||||
0.10134342312812805,
|
||||
0.3670985698699951,
|
||||
-0.9246448874473572,
|
||||
-0.41582486033439636
|
||||
],
|
||||
[
|
||||
0.07303514331579208,
|
||||
0.9241767525672913,
|
||||
0.37491756677627563,
|
||||
1.0754833221435547
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]])
|
||||
|
||||
def __init__(self, config_path):
|
||||
super().__init__(config_path)
|
||||
self.config_path = config_path
|
||||
self.robot_id = None
|
||||
self.turntable_id = None
|
||||
self.target_id = None
|
||||
camera_config = ConfigManager.get("simulation", "camera")
|
||||
self.camera_params = {
|
||||
'width': camera_config["width"],
|
||||
'height': camera_config["height"],
|
||||
'fov': camera_config["fov"],
|
||||
'near': camera_config["near"],
|
||||
'far': camera_config["far"]
|
||||
}
|
||||
self.sim_config = ConfigManager.get("simulation")
|
||||
|
||||
def run(self):
|
||||
print()
|
||||
def run(self, cmd):
|
||||
print(f"Simulator run {cmd}")
|
||||
if cmd == self.CREATE:
|
||||
self.prepare_env()
|
||||
self.create_env()
|
||||
elif cmd == self.SIMULATE:
|
||||
self.simulate()
|
||||
|
||||
def simulate(self):
|
||||
self.reset()
|
||||
self.init()
|
||||
debug_pose = Simulator.debug_pose
|
||||
offset = np.asarray([[1, 0, 0, 0], [0, -1, 0, 0], [0, 0, -1, 0], [0, 0, 0, 1]])
|
||||
debug_pose = debug_pose @ offset
|
||||
for _ in range(10000):
|
||||
debug_pose_2 = np.eye(4)
|
||||
debug_pose_2[0,0] = -1
|
||||
debug_pose_2[2,3] = 0.5
|
||||
self.move_to(debug_pose_2)
|
||||
# Wait for the system to stabilize
|
||||
for _ in range(20): # Simulate 20 steps to ensure stability
|
||||
p.stepSimulation()
|
||||
time.sleep(0.001) # Add small delay to ensure physics simulation
|
||||
|
||||
depth_img, segm_img = self.take_picture()
|
||||
p.stepSimulation()
|
||||
|
||||
def prepare_env(self):
|
||||
pass
|
||||
|
||||
p.connect(p.GUI)
|
||||
p.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
p.setGravity(0, 0, 0)
|
||||
p.loadURDF("plane.urdf")
|
||||
|
||||
def create_env(self):
|
||||
pass
|
||||
print(self.config)
|
||||
robot_config = self.sim_config["robot"]
|
||||
turntable_config = self.sim_config["turntable"]
|
||||
target_config = self.sim_config["target"]
|
||||
|
||||
self.robot_id = p.loadURDF(
|
||||
robot_config["urdf_path"],
|
||||
robot_config["initial_position"],
|
||||
p.getQuaternionFromEuler(robot_config["initial_orientation"]),
|
||||
useFixedBase=True
|
||||
)
|
||||
|
||||
p.changeDynamics(
|
||||
self.robot_id,
|
||||
linkIndex=-1,
|
||||
mass=0,
|
||||
linearDamping=0,
|
||||
angularDamping=0,
|
||||
lateralFriction=0
|
||||
)
|
||||
|
||||
visual_shape_id = p.createVisualShape(
|
||||
shapeType=p.GEOM_CYLINDER,
|
||||
radius=turntable_config["radius"],
|
||||
length=turntable_config["height"],
|
||||
rgbaColor=[0.7, 0.7, 0.7, 1]
|
||||
)
|
||||
collision_shape_id = p.createCollisionShape(
|
||||
shapeType=p.GEOM_CYLINDER,
|
||||
radius=turntable_config["radius"],
|
||||
height=turntable_config["height"]
|
||||
)
|
||||
self.turntable_id = p.createMultiBody(
|
||||
baseMass=0, # 设置质量为0使其成为静态物体
|
||||
baseCollisionShapeIndex=collision_shape_id,
|
||||
baseVisualShapeIndex=visual_shape_id,
|
||||
basePosition=turntable_config["center_position"]
|
||||
)
|
||||
|
||||
# 禁用转盘的动力学
|
||||
p.changeDynamics(
|
||||
self.turntable_id,
|
||||
-1, # -1 表示基座
|
||||
mass=0,
|
||||
linearDamping=0,
|
||||
angularDamping=0,
|
||||
lateralFriction=0
|
||||
)
|
||||
|
||||
|
||||
obj_path = os.path.join(target_config["obj_dir"], target_config["obj_name"], "mesh.obj")
|
||||
|
||||
assert os.path.exists(obj_path), f"Error: File not found at {obj_path}"
|
||||
|
||||
# 加载OBJ文件作为目标物体
|
||||
target_visual = p.createVisualShape(
|
||||
shapeType=p.GEOM_MESH,
|
||||
fileName=obj_path,
|
||||
rgbaColor=target_config["rgba_color"],
|
||||
specularColor=[0.4, 0.4, 0.4],
|
||||
meshScale=[target_config["scale"]] * 3
|
||||
)
|
||||
|
||||
# 使用简化的碰撞形状
|
||||
target_collision = p.createCollisionShape(
|
||||
shapeType=p.GEOM_MESH,
|
||||
fileName=obj_path,
|
||||
meshScale=[target_config["scale"]] * 3,
|
||||
flags=p.GEOM_FORCE_CONCAVE_TRIMESH # 尝试使用凹面网格
|
||||
)
|
||||
|
||||
|
||||
# 创建目标物体
|
||||
self.target_id = p.createMultiBody(
|
||||
baseMass=0, # 设置质量为0使其成为静态物体
|
||||
baseCollisionShapeIndex=target_collision,
|
||||
baseVisualShapeIndex=target_visual,
|
||||
basePosition=[
|
||||
turntable_config["center_position"][0],
|
||||
turntable_config["center_position"][1],
|
||||
turntable_config["height"] + turntable_config["center_position"][2]
|
||||
],
|
||||
baseOrientation=p.getQuaternionFromEuler([np.pi/2, 0, 0])
|
||||
)
|
||||
|
||||
# 禁用目标物体的动力学
|
||||
p.changeDynamics(
|
||||
self.target_id,
|
||||
-1, # -1 表示基座
|
||||
mass=0,
|
||||
linearDamping=0,
|
||||
angularDamping=0,
|
||||
lateralFriction=0
|
||||
)
|
||||
|
||||
# 创建固定约束，将目标物体固定在转盘上
|
||||
cid = p.createConstraint(
|
||||
parentBodyUniqueId=self.turntable_id,
|
||||
parentLinkIndex=-1, # -1 表示基座
|
||||
childBodyUniqueId=self.target_id,
|
||||
childLinkIndex=-1, # -1 表示基座
|
||||
jointType=p.JOINT_FIXED,
|
||||
jointAxis=[0, 0, 0],
|
||||
parentFramePosition=[0, 0, 0], # 相对于转盘中心的偏移
|
||||
childFramePosition=[0, 0, 0] # 相对于物体中心的偏移
|
||||
)
|
||||
|
||||
# 设置约束参数
|
||||
p.changeConstraint(cid, maxForce=100) # 设置最大力，确保约束稳定
|
||||
|
||||
def move_robot_to_pose(self, target_matrix):
|
||||
# 从4x4齐次矩阵中提取位置（前3个元素）
|
||||
position = target_matrix[:3, 3]
|
||||
|
||||
# 从3x3旋转矩阵中提取方向四元数
|
||||
R = target_matrix[:3, :3]
|
||||
|
||||
# 计算四元数的w分量
|
||||
w = np.sqrt(max(0, 1 + R[0,0] + R[1,1] + R[2,2])) / 2
|
||||
|
||||
# 避免除零错误，同时处理不同情况
|
||||
if abs(w) < 1e-8:
|
||||
# 当w接近0时的特殊情况
|
||||
x = np.sqrt(max(0, 1 + R[0,0] - R[1,1] - R[2,2])) / 2
|
||||
y = np.sqrt(max(0, 1 - R[0,0] + R[1,1] - R[2,2])) / 2
|
||||
z = np.sqrt(max(0, 1 - R[0,0] - R[1,1] + R[2,2])) / 2
|
||||
|
||||
# 确定符号
|
||||
if R[2,1] - R[1,2] < 0: x = -x
|
||||
if R[0,2] - R[2,0] < 0: y = -y
|
||||
if R[1,0] - R[0,1] < 0: z = -z
|
||||
else:
|
||||
# 正常情况
|
||||
x = (R[2,1] - R[1,2]) / (4 * w)
|
||||
y = (R[0,2] - R[2,0]) / (4 * w)
|
||||
z = (R[1,0] - R[0,1]) / (4 * w)
|
||||
|
||||
orientation = (x, y, z, w)
|
||||
|
||||
# 设置IK求解参数
|
||||
num_joints = p.getNumJoints(self.robot_id)
|
||||
lower_limits = []
|
||||
upper_limits = []
|
||||
joint_ranges = []
|
||||
rest_poses = []
|
||||
|
||||
# 获取关节限制和默认姿态
|
||||
for i in range(num_joints):
|
||||
joint_info = p.getJointInfo(self.robot_id, i)
|
||||
lower_limits.append(joint_info[8])
|
||||
upper_limits.append(joint_info[9])
|
||||
joint_ranges.append(joint_info[9] - joint_info[8])
|
||||
rest_poses.append(0) # 可以设置一个较好的默认姿态
|
||||
|
||||
# 使用增强版IK求解器，考虑碰撞避障
|
||||
joint_poses = p.calculateInverseKinematics(
|
||||
self.robot_id,
|
||||
7, # end effector link index
|
||||
position,
|
||||
orientation,
|
||||
lowerLimits=lower_limits,
|
||||
upperLimits=upper_limits,
|
||||
jointRanges=joint_ranges,
|
||||
restPoses=rest_poses,
|
||||
maxNumIterations=100,
|
||||
residualThreshold=1e-4
|
||||
)
|
||||
|
||||
# 分步移动到目标位置，同时检查碰撞
|
||||
current_poses = [p.getJointState(self.robot_id, i)[0] for i in range(7)]
|
||||
steps = 50 # 分50步移动
|
||||
|
||||
for step in range(steps):
|
||||
# 线性插值计算中间位置
|
||||
intermediate_poses = []
|
||||
for current, target in zip(current_poses, joint_poses):
|
||||
t = (step + 1) / steps
|
||||
intermediate = current + (target - current) * t
|
||||
intermediate_poses.append(intermediate)
|
||||
|
||||
# 设置关节位置
|
||||
for i in range(7):
|
||||
p.setJointMotorControl2(
|
||||
self.robot_id,
|
||||
i,
|
||||
p.POSITION_CONTROL,
|
||||
intermediate_poses[i]
|
||||
)
|
||||
|
||||
# 执行一步模拟
|
||||
p.stepSimulation()
|
||||
|
||||
# 检查碰撞
|
||||
if p.getContactPoints(self.robot_id, self.turntable_id):
|
||||
print("检测到潜在碰撞，停止移动")
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
|
||||
def rotate_turntable(self, angle_degrees):
|
||||
# 旋转转盘
|
||||
current_pos, current_orn = p.getBasePositionAndOrientation(self.turntable_id)
|
||||
current_orn = p.getEulerFromQuaternion(current_orn)
|
||||
|
||||
new_orn = list(current_orn)
|
||||
new_orn[2] += np.radians(angle_degrees)
|
||||
new_orn_quat = p.getQuaternionFromEuler(new_orn)
|
||||
|
||||
p.resetBasePositionAndOrientation(
|
||||
self.turntable_id,
|
||||
current_pos,
|
||||
new_orn_quat
|
||||
)
|
||||
|
||||
# 同时旋转目标物体
|
||||
target_pos, target_orn = p.getBasePositionAndOrientation(self.target_id)
|
||||
target_orn = p.getEulerFromQuaternion(target_orn)
|
||||
|
||||
# 更新目标物体的方向
|
||||
target_orn = list(target_orn)
|
||||
target_orn[2] += np.radians(angle_degrees)
|
||||
target_orn_quat = p.getQuaternionFromEuler(target_orn)
|
||||
|
||||
# 计算物体新的位置（绕转盘中心旋转）
|
||||
turntable_center = current_pos
|
||||
relative_pos = np.array(target_pos) - np.array(turntable_center)
|
||||
|
||||
# 创建旋转矩阵
|
||||
theta = np.radians(angle_degrees)
|
||||
rotation_matrix = np.array([
|
||||
[np.cos(theta), -np.sin(theta), 0],
|
||||
[np.sin(theta), np.cos(theta), 0],
|
||||
[0, 0, 1]
|
||||
])
|
||||
|
||||
# 计算新的相对位置
|
||||
new_relative_pos = rotation_matrix.dot(relative_pos)
|
||||
new_pos = np.array(turntable_center) + new_relative_pos
|
||||
|
||||
# 更新目标物体的位置和方向
|
||||
p.resetBasePositionAndOrientation(
|
||||
self.target_id,
|
||||
new_pos,
|
||||
target_orn_quat
|
||||
)
|
||||
|
||||
def get_camera_pose(self):
|
||||
end_effector_link = 7 # Franka末端执行器的链接索引
|
||||
state = p.getLinkState(self.robot_id, end_effector_link)
|
||||
ee_pos = state[0] # 世界坐标系中的位置
|
||||
camera_orn = state[1] # 世界坐标系中的朝向（四元数）
|
||||
|
||||
# 计算相机的视角矩阵
|
||||
rot_matrix = p.getMatrixFromQuaternion(camera_orn)
|
||||
rot_matrix = np.array(rot_matrix).reshape(3, 3)
|
||||
|
||||
# 相机的前向向量（与末端执行器的x轴对齐）
|
||||
camera_forward = rot_matrix.dot(np.array([0, 0, 1])) # x轴方向
|
||||
|
||||
# 将相机位置向前偏移0.1米
|
||||
offset = 0.12
|
||||
camera_pos = np.array(ee_pos) + camera_forward * offset
|
||||
camera_target = camera_pos + camera_forward
|
||||
|
||||
# 相机的上向量（与末端执行器的z轴对齐）
|
||||
camera_up = rot_matrix.dot(np.array([1, 0, 0])) # z轴方向
|
||||
|
||||
return camera_pos, camera_target, camera_up
|
||||
|
||||
def take_picture(self):
|
||||
camera_pos, camera_target, camera_up = self.get_camera_pose()
|
||||
|
||||
view_matrix = p.computeViewMatrix(
|
||||
cameraEyePosition=camera_pos,
|
||||
cameraTargetPosition=camera_target,
|
||||
cameraUpVector=camera_up
|
||||
)
|
||||
|
||||
projection_matrix = p.computeProjectionMatrixFOV(
|
||||
fov=self.camera_params['fov'],
|
||||
aspect=self.camera_params['width'] / self.camera_params['height'],
|
||||
nearVal=self.camera_params['near'],
|
||||
farVal=self.camera_params['far']
|
||||
)
|
||||
|
||||
_,_,rgb_img,depth_img,segm_img = p.getCameraImage(
|
||||
width=self.camera_params['width'],
|
||||
height=self.camera_params['height'],
|
||||
viewMatrix=view_matrix,
|
||||
projectionMatrix=projection_matrix,
|
||||
renderer=p.ER_BULLET_HARDWARE_OPENGL
|
||||
)
|
||||
|
||||
depth_img = self.camera_params['far'] * self.camera_params['near'] / (
|
||||
self.camera_params['far'] - (self.camera_params['far'] - self.camera_params['near']) * depth_img)
|
||||
|
||||
depth_img = np.array(depth_img)
|
||||
segm_img = np.array(segm_img)
|
||||
|
||||
return depth_img, segm_img
|
||||
|
||||
def reset(self):
|
||||
target_pos = [0.5, 0, 1]
|
||||
target_orn = p.getQuaternionFromEuler([np.pi, 0, 0])
|
||||
target_matrix = np.eye(4)
|
||||
target_matrix[:3, 3] = target_pos
|
||||
target_matrix[:3, :3] = np.asarray(p.getMatrixFromQuaternion(target_orn)).reshape(3,3)
|
||||
self.move_robot_to_pose(target_matrix)
|
||||
|
||||
def init(self):
|
||||
self.move_to(Simulator.INIT_GRIPPER_POSE)
|
||||
|
||||
def move_to(self, pose: np.ndarray):
|
||||
#delta_degree, min_new_cam_to_world = ControlUtil.solve_display_table_rot_and_cam_to_world(pose)
|
||||
#print(delta_degree)
|
||||
min_new_cam_to_pybullet_world = Simulator.TURNTABLE_WORLD_TO_PYBULLET_WORLD@pose
|
||||
self.move_to_cam_pose(min_new_cam_to_pybullet_world)
|
||||
#self.rotate_turntable(delta_degree)
|
||||
|
||||
|
||||
|
||||
def __del__(self):
|
||||
p.disconnect()
|
||||
|
||||
def create_experiment(self, backup_name=None):
|
||||
return super().create_experiment(backup_name)
|
||||
|
||||
def load_experiment(self, backup_name=None):
|
||||
super().load_experiment(backup_name)
|
||||
super().load_experiment(backup_name)
|
||||
|
||||
def move_to_cam_pose(self, camera_pose: np.ndarray):
|
||||
# 从相机位姿矩阵中提取位置和旋转矩阵
|
||||
camera_pos = camera_pose[:3, 3]
|
||||
R_camera = camera_pose[:3, :3]
|
||||
|
||||
# 相机的朝向向量（z轴）
|
||||
forward = R_camera[:, 2]
|
||||
|
||||
# 由于相机与末端执行器之间有固定偏移，需要计算末端执行器位置
|
||||
# 相机在末端执行器前方0.12米
|
||||
gripper_pos = camera_pos - forward * 0.12
|
||||
|
||||
# 末端执行器的旋转矩阵需要考虑与相机坐标系的固定变换
|
||||
# 假设相机的forward对应gripper的z轴，相机的x轴对应gripper的x轴
|
||||
R_gripper = R_camera
|
||||
|
||||
# 构建4x4齐次变换矩阵
|
||||
gripper_pose = np.eye(4)
|
||||
gripper_pose[:3, :3] = R_gripper
|
||||
gripper_pose[:3, 3] = gripper_pos
|
||||
print(gripper_pose)
|
||||
# 移动机器人到计算出的位姿
|
||||
return self.move_robot_to_pose(gripper_pose)
|
||||
59
utils/control.py
Normal file
59
utils/control.py
Normal file
@ -0,0 +1,59 @@
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as R
|
||||
import time
|
||||
|
||||
class ControlUtil:
|
||||
|
||||
curr_rotation = 0
|
||||
|
||||
@staticmethod
|
||||
def check_limit(new_cam_to_world):
|
||||
if new_cam_to_world[0,3] < 0 or new_cam_to_world[1,3] > 0:
|
||||
# if new_cam_to_world[0,3] > 0:
|
||||
return False
|
||||
x = abs(new_cam_to_world[0,3])
|
||||
y = abs(new_cam_to_world[1,3])
|
||||
tan_y_x = y/x
|
||||
min_angle = 0 / 180 * np.pi
|
||||
max_angle = 90 / 180 * np.pi
|
||||
if tan_y_x < np.tan(min_angle) or tan_y_x > np.tan(max_angle):
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
@staticmethod
|
||||
def solve_display_table_rot_and_cam_to_world(cam_to_world: np.ndarray) -> tuple:
|
||||
if ControlUtil.check_limit(cam_to_world):
|
||||
return 0, cam_to_world
|
||||
else:
|
||||
min_display_table_rot = 180
|
||||
min_new_cam_to_world = None
|
||||
for display_table_rot in np.linspace(0.1,360, 1800):
|
||||
new_world_to_world = ControlUtil.get_z_axis_rot_mat(display_table_rot)
|
||||
new_cam_to_new_world = cam_to_world
|
||||
new_cam_to_world = new_world_to_world @ new_cam_to_new_world
|
||||
|
||||
if ControlUtil.check_limit(new_cam_to_world):
|
||||
if display_table_rot < min_display_table_rot:
|
||||
min_display_table_rot, min_new_cam_to_world = display_table_rot, new_cam_to_world
|
||||
if abs(display_table_rot - 360) < min_display_table_rot:
|
||||
min_display_table_rot, min_new_cam_to_world = display_table_rot - 360, new_cam_to_world
|
||||
|
||||
if min_new_cam_to_world is None:
|
||||
raise ValueError("No valid display table rotation found")
|
||||
|
||||
delta_degree = min_display_table_rot - ControlUtil.curr_rotation
|
||||
ControlUtil.curr_rotation = min_display_table_rot
|
||||
return delta_degree, min_new_cam_to_world
|
||||
|
||||
@staticmethod
|
||||
def get_z_axis_rot_mat(degree):
|
||||
radian = np.radians(degree)
|
||||
return np.array([
|
||||
[np.cos(radian), -np.sin(radian), 0, 0],
|
||||
[np.sin(radian), np.cos(radian), 0, 0],
|
||||
[0, 0, 1, 0],
|
||||
[0, 0, 0, 1]
|
||||
])
|
||||
|
||||
|
||||
@ -70,7 +70,7 @@ class RenderUtil:
|
||||
|
||||
@staticmethod
|
||||
def render_pts(cam_pose, scene_path, script_path, scan_points, voxel_threshold=0.005, filter_degree=75, nO_to_nL_pose=None, require_full_scene=False):
|
||||
|
||||
import ipdb; ipdb.set_trace()
|
||||
nO_to_world_pose = DataLoadUtil.get_real_cam_O_from_cam_L(cam_pose, nO_to_nL_pose, scene_path=scene_path)
|
||||
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user