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)