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
