def abb_irb6640_180_255_robot(): """Return a Robot instance for the ABB IRB6640 180-255 robot""" x = np.array([1, 0, 0]) y = np.array([0, 1, 0]) z = np.array([0, 0, 1]) a = np.array([0, 0, 0]) H = np.array([z, y, y, x, y, x]).T P = np.array( [0.78 * z, 0.32 * x, 1.075 * z, 0.2 * z, 1.1425 * x, 0.2 * x, a]).T joint_type = [0, 0, 0, 0, 0, 0] joint_min = np.deg2rad(np.array([-170, -65, -180, -300, -120, -360])) joint_max = np.deg2rad(np.array([170, 85, 70, 300, 120, 360])) p_tool = np.array([0, 0, 0]) R_tool = rox.rot([0, 1, 0], np.pi / 2.0) return rox.Robot(H, P, joint_type, joint_min, joint_max, R_tool=R_tool, p_tool=p_tool)
def puma260b_robot(): """Returns an approximate Robot instance for a Puma 260B robot""" x = np.array([1, 0, 0]) y = np.array([0, 1, 0]) z = np.array([0, 0, 1]) a = np.array([0, 0, 0]) H = np.array([z, y, y, z, y, x]).T P = np.array( [13 * z, a, (-4.9 * y + 7.8 * x - 0.75 * z), -8.0 * z, a, a, 2.2 * x]).T * in_2_m joint_type = [0, 0, 0, 0, 0, 0] joint_min = np.deg2rad(np.array([-5, -256, -214, -384, -32, -267])) joint_max = np.deg2rad(np.array([313, 76, 34, 194, 212, 267])) return rox.Robot(H, P, joint_type, joint_min, joint_max)
def create_robot_rox(self): chains = self.robot_info.chains # Get RobotKinChainInfo self.H = chains[0].H # Axes of the joints, 3xN self.P = chains[ 0].P # P vectors between joint centers (Product of Exponenetials Convention) self.H_shaped = np.zeros((3, self.num_joints)) itr = 0 for i in self.H: self.H_shaped[:, itr] = (i[0], i[1], i[2]) itr += 1 self.P_shaped = np.zeros((3, self.num_joints + 1)) itr = 0 for i in self.P: self.P_shaped[:, itr] = (i[0], i[1], i[2]) itr += 1 # create the robot from toolbox # robot = rox.Robot(H,P,joint_types-1,joint_lower_limits,joint_upper_limits,joint_vel_limits,joint_acc_limits) self.robot_rox = rox.Robot(self.H_shaped, self.P_shaped, self.joint_types - 1)
def _robot_from_urdf_robot(urdf_robot, root_link = None, tip_link = None): if root_link is not None: assert root_link in urdf_robot.link_map, "Invalid root_link specified" else: root_link = urdf_robot.get_root() tip_links = [] valid_chains=[] for l1 in filter(lambda l: l not in urdf_robot.child_map, urdf_robot.link_map): try: chain = urdf_robot.get_chain(root_link, l1, True, False, True) if any(map(lambda c: urdf_robot.joint_map[c].joint_type == 'floating', chain)): continue if all(map(lambda c: urdf_robot.joint_map[c].joint_type == 'fixed', chain)): continue tip_links.append(l1) valid_chains.append(chain) except KeyError: pass if tip_link is None: assert len(tip_links) == 1, "Multiple robots detected, specify tip link of desired robot" tip_link = tip_links[0] chain = urdf_robot.get_chain(root_link, tip_link, True, False, True) assert len(chain) > 0, "Invalid robot chain found" n = len(list(filter(lambda c: urdf_robot.joint_map[c].joint_type != 'fixed', chain))) P = np.zeros((3, n+1)) H = np.zeros((3, n)) joint_type = np.zeros(n) joint_lower_limit = [None]*n joint_upper_limit = [None]*n joint_vel_limit = [None]*n joint_effort_limit = [None]*n joint_names = [None]*n M = [None]*(n+1) i = 0 R=np.identity(3) for c in chain: j = urdf_robot.joint_map[c] if j.origin is not None: if j.origin.xyz is not None: P[:,i] += R.dot(j.origin.xyz) if j.origin.rpy is not None: R = R.dot(_rpy_to_rot(j.origin.rpy)) if urdf_robot.link_map[j.child].inertial is not None: M[i] = _append_inertia(M[i], _convert_inertial(urdf_robot.link_map[j.parent].inertial,R)) if j.joint_type == 'fixed': pass elif j.joint_type == 'revolute' or j.joint_type == 'prismatic': H[:,i] = R.dot(j.axis) joint_type[i] = 0 if j.joint_type == 'revolute' else 1 if j.limit is not None: joint_lower_limit[i] = j.limit.lower joint_upper_limit[i] = j.limit.upper joint_vel_limit[i] = j.limit.velocity joint_effort_limit[i] = j.limit.effort joint_names[i] = j.name i += 1 else: assert False, "Only revolute, prismatic, fixed, and floating joints supported" M[-1] = _append_inertia(M[-1],_convert_inertial(urdf_robot.link_map[j.child].inertial,R)) if None in joint_lower_limit or None in joint_upper_limit: joint_lower_limit = None joint_upper_limit = None else: joint_lower_limit = np.array(joint_lower_limit) joint_upper_limit = np.array(joint_upper_limit) if None in joint_vel_limit: joint_vel_limit = None else: joint_vel_limit = np.array(joint_vel_limit) if np.allclose(np.zeros((3,3)), R, atol=1e-5): R_tool = None p_tool = None else: R_tool = R p_tool = np.zeros((3,)) if any([True for m1 in M if m1 is None]): M = None robot = rox.Robot(H, P, joint_type, joint_lower_limit, joint_upper_limit, \ joint_vel_limit, R_tool=R_tool, p_tool=p_tool, \ joint_names = joint_names, root_link_name = root_link, \ tip_link_name = tip_link, M = M ) # Tack on joint_effort_limit if available if not None in joint_effort_limit: robot.joint_effort_limit = joint_effort_limit return robot
def robot_info_to_rox_robot(self, robot_info, chain_number): _check_list(robot_info.chains, f"could not find kinematic chain number {chain_number}") if chain_number >= len(robot_info.chains): raise RR.InvalidArgumentException( f"invalid kinematic chain number {chain_number}") chain = robot_info.chains[chain_number] joint_count = len(chain.joint_numbers) for i in range(1, joint_count): if chain.joint_numbers[i - 1] >= chain.joint_numbers[i]: raise RR.InvalidArgumentException( f"joint numbers must be increasing in chain number {chain_number}" ) if chain.joint_numbers[i] >= len(robot_info.joint_info): raise RR.InvalidArgumentException( f"joint number out of bounds in chain number {chain_number}" ) _check_list(chain.H, f"invalid shape for H in chain number {chain_number}", joint_count) _check_list(chain.P, f"invalid shape for P in chain number {chain_number}", joint_count + 1) H = np.zeros((3, joint_count), dtype=np.float64) for i in range(joint_count): H[0, i] = chain.H[i]["x"] H[1, i] = chain.H[i]["y"] H[2, i] = chain.H[i]["z"] P = np.zeros((3, joint_count + 1), dtype=np.float64) for i in range(joint_count + 1): P[0, i] = chain.P[i]["x"] P[1, i] = chain.P[i]["y"] P[2, i] = chain.P[i]["z"] joint_type = [0] * joint_count joint_lower_limit = np.zeros((joint_count, ), dtype=np.float64) joint_upper_limit = np.zeros((joint_count, ), dtype=np.float64) joint_vel_limit = np.zeros((joint_count, ), dtype=np.float64) joint_acc_limit = np.zeros((joint_count, ), dtype=np.float64) joint_names = [None] * joint_count for i in range(joint_count): j = robot_info.joint_info[i] if j.joint_type == 1: # Revolute joint joint_type[i] = 0 elif j.joint_type == 3: # Prismatic joint joint_type[i] = 1 else: raise RR.InvalidArgumentException( f"invalid joint type: {j.joint_type}") if j.joint_limits is None: raise RR.InvalidArgumentException( "joint_limits must not be null") joint_lower_limit[i] = j.joint_limits.lower joint_upper_limit[i] = j.joint_limits.upper joint_vel_limit[i] = j.joint_limits.velocity joint_acc_limit[i] = j.joint_limits.acceleration if j.joint_identifier is not None: joint_names[i] = j.joint_identifier.name else: joint_names[i] = "" root_link_name = None if chain.link_identifiers is not None and len( chain.link_identifiers ) > 0 and chain.link_identifiers[0] is not None: root_link_name = chain.link_identifiers[0].name tip_link_name = None if chain.flange_identifier is not None: tip_link_name = chain.flange_identifier.name flange_q = chain.flange_pose["orientation"] flange_p = chain.flange_pose["position"] r_tool = rox.q2R(self._node.NamedArrayToArray(flange_q).flatten()) p_tool = np.array(self._node.NamedArrayToArray(flange_p).flatten()) rox_robot = rox.Robot(H, P, joint_type, joint_lower_limit, joint_upper_limit, joint_vel_limit, joint_acc_limit, None, r_tool, p_tool, joint_names, root_link_name, tip_link_name) return rox_robot
async def client_drive(): # rr+ws : WebSocket connection without encryption # url ='rr+ws://localhost:58653?service=sawyer' url = 'rr+ws://192.168.50.152:58653?service=sawyer' # url ='rr+ws://128.113.224.23:58654?service=sawyer' # sawyer in lab # url ='rr+ws://localhost:58655?service=robot' #ABB # url ='rr+ws://192.168.50.118:58655?service=robot' #ABB # url = 'rr+ws://localhost:23333?service=robot' # Dr.Wasons's Robot print_div('Program started, please wait..<br>') try: #Connect to the service global d # d is the robot object from RR d = await RRN.AsyncConnectService(url, None, None, None, None) # d.async_reset_errors(None) d.async_enable(None) # Define Robot modes global robot_const, halt_mode, jog_mode, position_mode, trajectory_mode robot_const = RRN.GetConstants("com.robotraconteur.robotics.robot", d) halt_mode = robot_const["RobotCommandMode"]["halt"] jog_mode = robot_const["RobotCommandMode"]["jog"] position_mode = robot_const["RobotCommandMode"]["velocity_command"] trajectory_mode = robot_const["RobotCommandMode"]["trajectory"] # Import Necessary Structures global JointTrajectoryWaypoint, JointTrajectory JointTrajectoryWaypoint = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint", d) JointTrajectory = RRN.GetStructureType( "com.robotraconteur.robotics.trajectory.JointTrajectory", d) # Put robot to jogging mode await d.async_set_command_mode(halt_mode, None, 5) await RRN.AsyncSleep(0.1, None) await d.async_set_command_mode(jog_mode, None, 5) await RRN.AsyncSleep(0.1, None) # # Put the robot to velocity conrol mode # await d.async_set_command_mode(halt_mode,None,5) # await RRN.AsyncSleep(1,None) # await d.async_set_command_mode(position_mode,None,5) # await RRN.AsyncSleep(1,None) # # Put robot to trajectory mode # await d.async_set_command_mode(halt_mode,None,5) # await RRN.AsyncSleep(0.1,None) # await d.async_set_command_mode(trajectory_mode,None,5) # await RRN.AsyncSleep(0.1,None) print_div('READY!<br>') global d_q # Get the current joint positions d_q = await update_joint_info( ) # Joint angles in radian ndarray, N x 1 global num_joints, joint_types, joint_lower_limits, joint_upper_limits, joint_vel_limits, joint_acc_limits, joint_names # Get the number of Joints, Joint Types, Limits etc in the robot. num_joints, joint_types, joint_lower_limits, joint_upper_limits, joint_vel_limits, joint_acc_limits, joint_names = await update_num_info( ) global H, P # Get the kinematics info, P and H in product of exponentials convention H, P = await update_kin_info() # print_div_end_info(str(H)) global robot # Robotics Toolbox Robot Object (NOT THE RR ROBOT OBJECT, IT IS d) # Now we are ready to create the robot from toolbox robot = rox.Robot(H, P, joint_types - 1, joint_lower_limits, joint_upper_limits, joint_vel_limits, joint_acc_limits) # robot = rox.Robot(H,P,joint_types-1) global pose # Current pose object from Robotics Toolbox of the end effector # UPdate the end effector pose info pose = await update_end_info() # Element references # Joint Space Control Buttons button_stop = document.getElementById("stop_btn") button_j1_pos = document.getElementById("j1_pos_btn") button_j1_neg = document.getElementById("j1_neg_btn") button_j2_pos = document.getElementById("j2_pos_btn") button_j2_neg = document.getElementById("j2_neg_btn") button_j3_pos = document.getElementById("j3_pos_btn") button_j3_neg = document.getElementById("j3_neg_btn") button_j4_pos = document.getElementById("j4_pos_btn") button_j4_neg = document.getElementById("j4_neg_btn") button_j5_pos = document.getElementById("j5_pos_btn") button_j5_neg = document.getElementById("j5_neg_btn") button_j6_pos = document.getElementById("j6_pos_btn") button_j6_neg = document.getElementById("j6_neg_btn") button_j7_pos = document.getElementById("j7_pos_btn") button_j7_neg = document.getElementById("j7_neg_btn") button_angles_submit = document.getElementById("j_angles_submit_btn") # Task Space Control Buttons button_X_pos = document.getElementById("X_pos_btn") button_X_neg = document.getElementById("X_neg_btn") button_Y_pos = document.getElementById("Y_pos_btn") button_Y_neg = document.getElementById("Y_neg_btn") button_Z_pos = document.getElementById("Z_pos_btn") button_Z_neg = document.getElementById("Z_neg_btn") button_theta_X_pos = document.getElementById("theta_X_pos_btn") button_theta_X_neg = document.getElementById("theta_X_neg_btn") button_theta_Y_pos = document.getElementById("theta_Y_pos_btn") button_theta_Y_neg = document.getElementById("theta_Y_neg_btn") button_theta_Z_pos = document.getElementById("theta_Z_pos_btn") button_theta_Z_neg = document.getElementById("theta_Z_neg_btn") # Move robot to a certain default position # move_to_angles_func(None) # Playback Poses Buttons button_save_cur_pose = document.getElementById("save_pose_btn") button_go_sel_pose = document.getElementById("go_sel_pose_btn") button_playback_poses = document.getElementById("playback_poses_btn") button_stop.addEventListener("click", stop_func) button_j1_pos.addEventListener("mousedown", j1_pos_func) button_j1_neg.addEventListener("mousedown", j1_neg_func) button_j2_pos.addEventListener("mousedown", j2_pos_func) button_j2_neg.addEventListener("mousedown", j2_neg_func) button_j3_pos.addEventListener("mousedown", j3_pos_func) button_j3_neg.addEventListener("mousedown", j3_neg_func) button_j4_pos.addEventListener("mousedown", j4_pos_func) button_j4_neg.addEventListener("mousedown", j4_neg_func) button_j5_pos.addEventListener("mousedown", j5_pos_func) button_j5_neg.addEventListener("mousedown", j5_neg_func) button_j6_pos.addEventListener("mousedown", j6_pos_func) button_j6_neg.addEventListener("mousedown", j6_neg_func) button_j7_pos.addEventListener("mousedown", j7_pos_func) button_j7_neg.addEventListener("mousedown", j7_neg_func) button_angles_submit.addEventListener("click", move_to_angles_func) button_X_pos.addEventListener("mousedown", X_pos_func) button_X_neg.addEventListener("mousedown", X_neg_func) button_Y_pos.addEventListener("mousedown", Y_pos_func) button_Y_neg.addEventListener("mousedown", Y_neg_func) button_Z_pos.addEventListener("mousedown", Z_pos_func) button_Z_neg.addEventListener("mousedown", Z_neg_func) button_theta_X_pos.addEventListener("mousedown", tX_pos_func) button_theta_X_neg.addEventListener("mousedown", tX_neg_func) button_theta_Y_pos.addEventListener("mousedown", tY_pos_func) button_theta_Y_neg.addEventListener("mousedown", tY_neg_func) button_theta_Z_pos.addEventListener("mousedown", tZ_pos_func) button_theta_Z_neg.addEventListener("mousedown", tZ_neg_func) button_save_cur_pose.addEventListener("click", save_cur_pose_func) button_go_sel_pose.addEventListener("click", go_sel_pose_func) button_playback_poses.addEventListener("click", playback_poses_func) global is_jogging is_jogging = False global is_mousedown is_mousedown = False document.addEventListener("mouseup", mouseup_func) global is_gamepadbuttondown is_gamepadbuttondown = False global is_gamepadaxisactive is_gamepadaxisactive = False while True: # Update joint angles d_q = await update_joint_info() # Joint angles in radian ndarray # UPdate the end effector pose info pose = await update_end_info() await update_state_flags() except: import traceback print_div(traceback.format_exc()) raise