def geometry_pose_inv(pose):
"""
Invert a pose
Parameters:
pose (Pose): The pose to invert
Return (Pose): The inverted pose
"""
geom_util = GeometryUtil(node = PyriSandboxContext.node)
T_rox = geom_util.pose_to_rox_transform(_convert_to_pose(pose))
T_res = T_rox.inv()
return geom_util.rox_transform_to_pose(T_res)
def robot_get_end_pose(frame):
"""
Returns the end pose of a robot. This end pose is reported by the
robot driver. It is typically defined as the flange of the robot,
but may be the tool if the driver is configured to report
the tool pose.
Parameters:
* frame (str): The frame to return the pose in. May be `robot` or `world`.
Return (Pose): The robot end pose in the requested frame
"""
robot_name = _get_active_robot_name()
device_manager = PyriSandboxContext.device_manager
robot = device_manager.get_device_client(robot_name,1)
robot_state, _ = robot.robot_state.PeekInValue()
robot_util = RobotUtil(client_obj = robot)
geom_util = GeometryUtil(client_obj = robot)
# TODO: cache robot_info
rox_robot = robot_util.robot_info_to_rox_robot(robot.robot_info,0)
T1 = rox.fwdkin(rox_robot,robot_state.joint_position)
if frame =="ROBOT":
return geom_util.rox_transform_to_pose(T1)
elif frame == "WORLD":
var_storage = device_manager.get_device_client("variable_storage")
# TODO: don't hard code robot origin
robot_origin_pose = var_storage.getf_variable_value("globals",_get_robot_origin_calibration()).data
T_rob = geom_util.named_pose_to_rox_transform(robot_origin_pose.pose)
T2 = T_rob * T1
return geom_util.rox_transform_to_pose(T2)
else:
assert False, "Invalid frame"
def geometry_pose_multiply(pose_a, pose_b):
"""
Multiply one pose with another
Parameters:
* pose_a (Pose): The first pose
* pose_b (Pose): The second pose
Return (Pose): The result of the multiplication
"""
geom_util = GeometryUtil(node = PyriSandboxContext.node)
T_a = geom_util.pose_to_rox_transform(_convert_to_pose(pose_a))
T_b = geom_util.pose_to_rox_transform(_convert_to_pose(pose_b))
T_res = T_a * T_b
return geom_util.rox_transform_to_pose(T_res)
def movel(self,
robot_local_device_name,
pose_final,
frame,
robot_origin_calib_global_name,
speed_perc,
final_seed=None):
robot = self.device_manager.get_device_client(robot_local_device_name)
geom_util = GeometryUtil(client_obj=robot)
if frame.lower() == "world":
var_storage = self.device_manager.get_device_client(
"variable_storage")
robot_origin_pose = var_storage.getf_variable_value(
"globals", robot_origin_calib_global_name).data
T_rob = geom_util.named_pose_to_rox_transform(
robot_origin_pose.pose)
T_des1 = geom_util.pose_to_rox_transform(pose_final)
T_des = T_rob.inv() * T_des1
pose_final = geom_util.rox_transform_to_pose(T_des)
elif frame.lower() == "robot":
T_des = geom_util.pose_to_rox_transform(pose_final)
else:
assert False, "Unknown parent frame for movel"
robot_info = robot.robot_info
rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0)
robot_state = robot.robot_state.PeekInValue()[0]
q_initial = robot_state.joint_position
traj = self._generate_movel_trajectory(robot, rox_robot, q_initial,
T_des, speed_perc, final_seed)
if traj is None:
return EmptyGenerator()
return TrajectoryMoveGenerator(robot, rox_robot, traj, self._node)
d.connect_device("robot")
c = d.get_device_client("robotics_motion", 1)
#p_target = np.array([-np.random.uniform(0.4,0.8),np.random.uniform(-0.1,0.1),np.random.uniform(0.0,0.4)])
p_target = np.array([
-np.random.uniform(-0.1, 0.1),
np.random.uniform(-0.1, 0.1),
np.random.uniform(0.0, 0.4)
])
rpy_target = np.random.randn(3) * 0.5
rpy_target[0] += np.pi
R_target = rox.rpy2R(rpy_target)
# p_target = np.array([-0.6, 0.0, 0.1])
# R_target = np.array([[0,1,0],[1,0,0],[0,0,-1]])
T_target = rox.Transform(R_target, p_target)
r = d.get_device_client("robot", 1)
geom_util = GeometryUtil(client_obj=r)
p_target = geom_util.rox_transform_to_pose(T_target)
print("Begin movel")
gen = c.movel("robot", p_target, "world", "robot_origin_calibration", 50)
while True:
try:
gen.Next()
except RR.StopIterationException:
break
print("End movel")
print(jog_service.device_info.device.name)
jog = jog_service.get_jog("robot")
jog.setf_jog_mode()
#for x in range(100):
#jog.jog_joints3(1,1)
#jog.setf_halt_mode()
robot = d.get_device_client("robot", 1)
robot_state, _ = robot.robot_state.PeekInValue()
q_current = robot_state.joint_position
robot_util = RobotUtil(client_obj=robot)
rox_robot = robot_util.robot_info_to_rox_robot(robot.robot_info, 0)
geom_util = GeometryUtil(client_obj=jog_service)
T = rox.fwdkin(rox_robot, q_current)
print(f"Current xyz = {T.p}, rpy = {np.rad2deg(rox.R2rpy(T.R))}")
T2 = copy.deepcopy(T)
T2.p[1] += 0.1
T3 = copy.deepcopy(T)
T3.p[1] -= 0.1
pose_des = geom_util.rox_transform_to_pose(T2)
pose_des2 = geom_util.rox_transform_to_pose(T3)
for i in range(10):
jog.jog_joints_to_pose(pose_des, 50)
jog.jog_joints_to_pose(pose_des2, 50)