def teleport(self, location, rotation, settle=False):
if self.ar is None:
self.ar = self.dc.get_rigid_body(
self.robot_prim.GetPath().pathString + "/Vehicle")
self.chassis = self.ar
self.dc.wake_up_rigid_body(self.ar)
rot_quat = Gf.Rotation(Gf.Vec3d(0, 0, 1), rotation).GetQuaternion()
tf = _dynamic_control.Transform(
location,
(rot_quat.GetImaginary()[0], rot_quat.GetImaginary()[1],
rot_quat.GetImaginary()[2], rot_quat.GetReal()),
)
self.dc.set_rigid_body_pose(self.chassis, tf)
self.dc.set_rigid_body_linear_velocity(self.chassis, [0, 0, 0])
self.dc.set_rigid_body_angular_velocity(self.chassis, [0, 0, 0])
self.command((0, 0))
if settle:
frame = 0
velocity = 1
print("Settling robot...")
while velocity > 0.1 and frame < 120:
self.omni_kit.update(1.0 / 60.0)
lin_vel = self.dc.get_rigid_body_linear_velocity(self.chassis)
velocity = np.linalg.norm([lin_vel.x, lin_vel.y, lin_vel.z])
# print("velocity magnitude is: ", velocity)
frame = frame + 1
def get_target_to_object(self, offset_position=[]):
"""
Gets target pose to end effector on a given target, with an offset on the end effector actuator direction given
by [offset_up, offset_down]
"""
offset = _dynamic_control.Transform()
if offset_position:
offset.p.x = offset_position[0]
offset.p.y = offset_position[1]
offset.p.z = offset_position[2]
body_handle = self.dc.get_rigid_body(self.current)
obj_pose = self.dc.get_rigid_body_pose(body_handle)
target_pose = _dynamic_control.Transform()
target_pose.p = obj_pose.p
target_pose.r = self.get_target_orientation()
target_pose = math_utils.mul(target_pose, offset)
target_pose.p = math_utils.mul(target_pose.p, 0.01)
return target_pose
def _grasping_attached(self, *args):
self.waypoints.clear()
offset = _dynamic_control.Transform()
offset.p.z = -10
target_pose = math_utils.mul(self.get_current_state_tr(), offset)
target_pose.p = math_utils.mul(target_pose.p, 0.01)
self.lerp_to_pose(target_pose, n_waypoints=60)
self.lerp_to_pose(target_pose, n_waypoints=120)
# Move to next state
self.move_to_target()
self.robot.end_effector.gripper.width_history.clear()
self.change_state(SM_states.LIFTING)
def create_franka(self, *args):
super().create_franka()
if self.asset_path is None:
return
self.objects = [
self.asset_path +
"/Props/Flip_Stack/large_corner_bracket_physics.usd",
self.asset_path + "/Props/Flip_Stack/screw_95_physics.usd",
self.asset_path + "/Props/Flip_Stack/screw_99_physics.usd",
self.asset_path +
"/Props/Flip_Stack/small_corner_bracket_physics.usd",
self.asset_path + "/Props/Flip_Stack/t_connector_physics.usd",
]
self.current_obj = 0
# Load robot environment and set its transform
self.env_path = "/scene"
robot_usd = self.asset_path + "/Robots/Franka/franka.usd"
robot_path = "/scene/robot"
create_prim_from_usd(self._stage, robot_path, robot_usd,
Gf.Vec3d(0, 0, 0))
# Set robot end effector Target
target_path = "/scene/target"
if self._stage.GetPrimAtPath(target_path):
return
GoalPrim = self._stage.DefinePrim(target_path, "Xform")
self.default_position = _dynamic_control.Transform()
self.default_position.p = [0.4, 0.0, 0.3]
self.default_position.r = [0.0, 1.0, 0.0,
0.0] #TODO: Check values for stability
p = self.default_position.p
r = self.default_position.r
set_translate(GoalPrim, Gf.Vec3d(p.x * 100, p.y * 100, p.z * 100))
set_rotate(GoalPrim, Gf.Matrix3d(Gf.Quatd(r.w, r.x, r.y, r.z)))
# Setup physics simulation
add_ground_plane(self._stage, "/groundPlane", "Z", 1000.0,
Gf.Vec3f(0.0), Gf.Vec3f(1.0))
setup_physics(self._stage)
self.add_bin()
def get_current_state_tr(self):
"""
Gets current End Effector Transform, converted from Motion position and Rotation matrix
"""
# Gets end effector frame
state = self.robot.end_effector.status.current_frame
orig = state["orig"] * 100.0
mat = Gf.Matrix3f(*state["axis_x"].astype(float),
*state["axis_y"].astype(float),
*state["axis_z"].astype(float))
q = mat.ExtractRotation().GetQuaternion()
(q_x, q_y, q_z) = q.GetImaginary()
q = [q_x, q_y, q_z, q.GetReal()]
tr = _dynamic_control.Transform()
tr.p = list(orig)
tr.r = q
return tr