def _test_last_configuration(robot, theta, last_theta):
pose_1 = rox.fwdkin(robot, theta)
theta2 = rox.robot6_sphericalwrist_invkin(robot, pose_1, last_theta)
pose_2 = rox.fwdkin(robot, theta2[0])
if not pose_1 == pose_2:
return False
if not np.allclose(theta2[0], last_theta, atol=np.deg2rad(10)):
return False
return True
def _test_configuration(robot, theta):
pose_1 = rox.fwdkin(robot, theta)
theta2 = rox.robot6_sphericalwrist_invkin(robot, pose_1)
if not len(theta2) > 0:
return False
for theta2_i in theta2:
pose_2 = rox.fwdkin(robot, theta2_i)
if not pose_1 == pose_2:
return False
return True
def runTest(self):
#TODO: other joint types
#Home configuration (See Page 2-2 of Puma 260 manual)
puma = puma260b_robot()
pose = rox.fwdkin(puma, np.zeros(6))
np.testing.assert_allclose(pose.R, np.identity(3))
np.testing.assert_allclose(pose.p,
np.array([10, -4.9, 4.25]) * in_2_m,
atol=1e-6)
#Another right-angle configuration
joints2 = np.array([180, -90, -90, 90, 90, 90]) * np.pi / 180.0
pose2 = rox.fwdkin(puma, joints2)
np.testing.assert_allclose(pose2.R,
rox.rot([0, 0, 1], np.pi).dot(
rox.rot([0, 1, 0], -np.pi / 2)),
atol=1e-6)
np.testing.assert_allclose(pose2.p,
np.array([-0.75, 4.9, 31]) * in_2_m)
#Random configuration
joints3 = np.array([50, -105, 31, 4, 126, -184]) * np.pi / 180
pose3 = rox.fwdkin(puma, joints3)
pose3_R_t=np.array([[0.4274, 0.8069, -0.4076],\
[0.4455, -0.5804,-0.6817], \
[-0.7866, 0.1097, -0.6076]])
pose3_p_t = [0.2236, 0.0693, 0.4265]
np.testing.assert_allclose(pose3.R, pose3_R_t, atol=1e-4)
np.testing.assert_allclose(pose3.p, pose3_p_t, atol=1e-4)
puma_tool = puma260b_robot_tool()
pose4 = rox.fwdkin(puma_tool, joints3)
pose4_R_t=np.array([[0.4076, 0.8069, 0.4274],\
[0.6817, -0.5804,0.4455], \
[0.6076, 0.1097, -0.7866]])
pose4_p_t = [0.2450, 0.0916, 0.3872]
np.testing.assert_allclose(pose4.R, pose4_R_t, atol=1e-4)
np.testing.assert_allclose(pose4.p, pose4_p_t, atol=1e-4)
def update_qdot2(
self, R_axis, P_axis, speed_perc
): # inverse velocity kinematics that uses LSQ Linear solver
# Get the corresponding joint angles at that time
d_q = self.get_current_joint_positions()
q_cur = d_q.reshape((self.num_joints, 1))
# Update the end effector pose info
pose = rox.fwdkin(self.robot_rox, q_cur.flatten())
R_cur = pose.R
p_cur = pose.p
#calculate current Jacobian
J0T = rox.robotjacobian(self.robot_rox, q_cur.flatten())
# Transform Jacobian to End effector frame from the base frame
Tr = np.zeros((6, 6))
Tr[:3, :3] = R_cur.T
Tr[3:, 3:] = R_cur.T
#J0T = Tr @ J0T
# Normalize R_axis and P_axis
#R_axis = R_axis/(np.linalg.norm(R_axis))
#P_axis = P_axis/(np.linalg.norm(P_axis))
# Create the corresponding velocities
w = R_axis #* self.rotate_angle
v = P_axis #* self.move_distance
b = np.concatenate([w, v]) * 0.01 * speed_perc
np.nan_to_num(b, copy=False, nan=0.0, posinf=None, neginf=None)
# print(b)
# print(J0T)
joint_vel_limits = 0.01 * speed_perc * self.joint_vel_limits
res = lsq_linear(J0T,
b,
bounds=(-1.0 * joint_vel_limits, joint_vel_limits))
if res.success:
qdot_star = res.x
else:
print("Any solution could not found")
qdot_star = np.zeros(self.num_joints)
print("qdot_star:")
print(qdot_star)
# print("self.joint_vel_limits")
# print(self.joint_vel_limits)
# q_dot = self.normalize_dq(qdot_star)
q_dot = qdot_star
return q_dot
async def update_end_info():
global robot
global d_q
pose = rox.fwdkin(robot, d_q)
print_div_end_info(str(pose))
# print_div_end_info( np.array_str(pose.R, precision=4, suppress_small=True).replace('\n', '\n' + ' '*4))
# Calculate euler ZYX angles from pose and write them into:
x, y, z = euler_angles_from_rotation_matrix(pose.R)
str_rx = "%.2f" % (np.rad2deg(x))
str_ry = "%.2f" % (np.rad2deg(y))
str_rz = "%.2f" % (np.rad2deg(z))
str_px = "%.3f" % (pose.p[0])
str_py = "%.3f" % (pose.p[1])
str_pz = "%.3f" % (pose.p[2])
xyz_orient_str = "[" + str_rx + ", " + str_ry + ", " + str_rz + "]"
xyz_positi_str = "[" + str_px + ", " + str_py + ", " + str_pz + "]"
print_div_cur_pose(xyz_orient_str, xyz_positi_str)
return pose
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 compute_joint_vel_cmd_qp(self, joint_position,
spatial_velocity_command):
pp, RR = self.fwdkin_alljoints(joint_position)
## desired eef orientation ##
if self._eq_orien == True:
self._R_des = RR[:, :, -1]
self._eq_orien = False
## if the self-collision has higher priority than collision with environment
#if self._self_min_dist <= self._min_dist:
# Closest_Pt = self._self_Closest_Pt_1
# Closest_Pt_env = self._self_Closest_Pt_2
#else:
# Closest_Pt = self._Closest_Pt
# Closest_Pt_env = self._Closest_Pt_env
#Closest_Pt = self._Closest_Pt
#Closest_Pt_env = self._Closest_Pt_env
# where is the closest joint to the closest point
#J2C_Joint = self.Joint2Collision(Closest_Pt, pp)
# jacobian of end-effector
J_eef = rox.robotjacobian(self._robot, joint_position)
#v_tmp = Closest_Pt - pp[:, [-1]]
#v_tmp2 = (pp[:, [-1]] - pp[:, [-3]])
#p_norm2 = norm(v_tmp2)
#v_tmp2 = v_tmp2/p_norm2
# desired rotational velocity
vr = spatial_velocity_command[0:3]
vr = vr.reshape(3, 1)
# desired linear velocity
vp = spatial_velocity_command[3:None]
vp = vp.reshape(3, 1)
#J = self.getJacobian3(joint_position, Closest_Pt)
##### change 6 #####
#J, _ = self.getJacobian2(joint_position, Closest_Pt, 6)
### change J to J_eef ###
Q = self.getqp_H(J_eef, vr, vp)
# make sure Q is symmetric
Q = 0.5 * (Q + Q.T)
f = self.getqp_f()
f = f.reshape((8, ))
### change the velocity scale ###
LB = np.vstack((-self._robot.joint_vel_limit.reshape(6, 1), 0, 0))
UB = np.vstack((self._robot.joint_vel_limit.reshape(6, 1), 1, 1))
# inequality constrains A and b
#self._h[0:6] = joint_position.reshape(6, 1) - self._robot.joint_lower_limit.reshape(6, 1)
#self._h[6:12] = self._robot.joint_upper_limit.reshape(6, 1) - joint_position.reshape(6, 1)
#dx = Closest_Pt_env[0] - Closest_Pt[0]
#dy = Closest_Pt_env[1] - Closest_Pt[1]
#dz = Closest_Pt_env[2] - Closest_Pt[2]
#dist = np.sqrt(dx**2 + dy**2 + dz**2)
#dist = norm(Closest_Pt-Closest_Pt_env)
# derivative of dist w.r.t time
#der = np.array([dx*(dx**2 + dy**2 + dz**2)**(-0.5), dy*(dx**2 + dy**2 + dz**2)**(-0.5), dz*(dx**2 + dy**2 + dz**2)**(-0.5)])
#print dist
#dmin = 0.03#0.115
#self._h[12] = dist - dmin
#self._h[12] = 0.5*(dist*dist - dmin*dmin)
## change here ##
#self._dhdq[12, 0:6] = np.dot(-der.T, J[3:6,:])
#self._dhdq[12, 0:6] = np.dot(-dist*der.T, J[3:6,:])
#self._dhdq[12, 0:6] = np.dot(-Closest_Pt_env.T+Closest_Pt.T, J[3:6,:])
#self._sigma[0:12] = self.inequality_bound(self._h[0:12])
#self._sigma[12] = self.inequality_bound(self._h[12])
#print self._h[12]
#print self._sigma[12]
#A = self._dhdq
#b = self._sigma
#A = np.vstack((self._dhdq, np.eye(8), -np.eye(8)))
#b = np.vstack((self._sigma, LB, -UB))
#b = b.reshape((29, ))
A = np.vstack((np.eye(8), -np.eye(8)))
b = np.vstack((LB, -UB))
b = b.reshape((16, ))
# equality constraints for maintaining end-effector orientation (pure translation)
#A_eq = np.hstack((J_eef[0:3,:], np.zeros((3, 2))))
#w_skew = logm(np.dot(RR[:,:,-1], self._R_des.T))
#w = np.array([w_skew[2, 1], w_skew[0, 2], w_skew[1, 0]])
######### change -0.05 ##########
#b_eq = -0.001*self._Ke*w
# stack equality constrains on top of the inequality constraints
#A = np.vstack((A_eq, A))
#b = np.concatenate((b_eq.reshape((3, 1)), b.reshape((29, 1))), axis=0)
#b = b.reshape((32, ))
# the last argument specify the number of equality constraints
#sc = norm(Q,'fro')
#dq_sln = quadprog.solve_qp(Q/sc, -f/sc, A.T, b, A_eq.shape[0])[0]
#A = np.delete(A, [0, 1, 2], axis=0)
#b = np.delete(b, [0, 1, 2])
# solve the quadprog problem
## scale the matrix to avoid numerical errors of solver
sc = norm(Q, 'fro')
dq_sln = quadprog.solve_qp(Q / sc, -f / sc, A.T, b)[0]
#dq_sln = quadprog.solve_qp(Q, -f, A.T, b)[0]
if len(dq_sln) < self._n:
qdot = np.zeros((self._n, 1))
V_scaled = 0
print 'No Solution'
else:
qdot = dq_sln[0:self._n]
V_scaled = dq_sln[-1] * vp
#vr_scaled = dq_sln[-2]*vr.reshape(3,1)
V_linear = np.dot(J_eef[3:6, :], qdot)
V_rot = np.dot(J_eef[0:3, :], qdot)
qdot = qdot.reshape((6, ))
#print 'desired angular velocity'
print vr
#print 'actual angular velocity'
print V_rot
#print 'desired linear velocity'
print vp
#print 'actual linear velocity'
print V_linear
#print 'solved joint velocity'
#print qdot
####### for Rviz visualization ######
##### 2
br = tf.TransformBroadcaster()
pub_d = rospy.Publisher('desired_velocity',
TwistStamped,
queue_size=10)
pub_a = rospy.Publisher('actual_velocity', TwistStamped, queue_size=10)
msg_d = TwistStamped()
msg_a = TwistStamped()
transform_0T = rox.fwdkin(self._robot, joint_position)
#p = transform_0T.p
#print p
#q_orig1 = quaternion_from_euler(spatial_velocity_command[0], spatial_velocity_command[1], spatial_velocity_command[2])
#q = quaternion_from_euler(-euler[0], -euler[1], -euler[2])
# first element is w
q_orig2 = Quaternion(matrix=transform_0T.R)
#print q_orig2
# convert q_orig2 into the form that w is the last element
# inverse the quaternion by inverting w
q_orig3 = np.array([q_orig2[1], q_orig2[2], q_orig2[3], -q_orig2[0]])
#q_orig = np.array([0, 1, 0, 0])
#q_orig5 = quaternion_multiply(q_orig3, q_orig4)
# create a frame for velocity
br.sendTransform((0.0, 0.0, -0.2), q_orig3, rospy.Time.now(), "v_d",
"vacuum_gripper_tool")
br.sendTransform((0.0, 0.2, 0.0), (0, 0, 0, 1), rospy.Time.now(),
"v_a", "v_d")
msg_d.header.seq = 1
msg_d.header.stamp = rospy.Time.now()
msg_d.header.frame_id = "v_d"
msg_d.twist.linear.x = vp[0] # + p[0]
msg_d.twist.linear.y = vp[1] # + p[1]
msg_d.twist.linear.z = vp[2] # + p[2]
# the last one should be w
#q_orig = quaternion_multiply(q_orig1, q_orig5)
msg_d.twist.angular.x = vr[0] #q_orig[0]
msg_d.twist.angular.y = vr[1] #q_orig[1]
msg_d.twist.angular.z = vr[2] #q_orig[2]
msg_a.header.seq = 1
msg_a.header.stamp = rospy.Time.now()
msg_a.header.frame_id = "v_a"
msg_a.twist.linear.x = V_linear[0] # + p[0]
msg_a.twist.linear.y = V_linear[1] # + p[1]
msg_a.twist.linear.z = V_linear[2] # + p[2]
# the last one should be w
#q_orig = quaternion_multiply(q_orig1, q_orig5)
msg_a.twist.angular.x = V_rot[0] #q_orig[0]
msg_a.twist.angular.y = V_rot[1] #q_orig[1]
msg_a.twist.angular.z = V_rot[2] #q_orig[2]
#msg.pose.orientation.w = q_orig[3]
pub_d.publish(msg_d)
pub_a.publish(msg_a)
###################
return qdot
def compute_quadprog(self, joint_position):
self._tic = timeit.default_timer()
with self._lock:
print '-------force-------'
print self._F
print '-------distance between eef and panel-------'
print self._dist
self._F_new = self._F
force_err = self._F - self._fd
self._force_inte += force_err
self._force_der = (self._F_new - self._F_old) / (
self._tic - self._toc) # derivative part
# P control to approach the target
vz = -self._kp * force_err # - self._ki*self._force_inte-self._kd*self._force_der; # velocity in tool0 frame
# change velocity in world frame
try:
# rot is a quaternion (x, y, z, w)
(trans,
rot) = self._listener.lookupTransform('/base_link',
'/gripper',
rospy.Time(0))
except (LookupException, ConnectivityException,
ExtrapolationException):
pass
# convert quaternion into (w, x, y, z)
# convert from quaternion to rotation matrix (4 by 4)
R_rot = quaternion_matrix([rot[3], rot[0], rot[1], rot[2]])
R_rot = R_rot[0:3, 0:3]
# if there is no contact, only move toward the target
if self._F == 0:
self._kp = 0.000004
# 3 by 1 linear velocity in tool0 frame
v_l = np.array([0, 0, vz])
# if there is contact, also move in x direction in sensor frame
else:
# increase Kp gain when in contact with the target
self._kp = 0.000007
# PID control
vz = -self._kp * force_err - self._ki * self._force_inte - self._kd * self._force_der
# 3 by 1 linear velocity in tool0 frame
v_l = np.array([-0.003, 0, vz])
# transform into base frame
v_l = np.dot(R_rot, v_l)
# make a complete 6 by 1 velocity command with zero angular velocity
spatial_velocity_command = np.array(
[0, 0, 0, v_l[0], v_l[1], v_l[2]])
####### for Rviz visualization ######
##### 2
br = tf.TransformBroadcaster()
pub_v = rospy.Publisher('resolved_velocity',
TwistStamped,
queue_size=10)
msg_v = TwistStamped()
transform_0T = rox.fwdkin(self._robot, joint_position)
#p = transform_0T.p
#print p
#q_orig1 = quaternion_from_euler(spatial_velocity_command[0], spatial_velocity_command[1], spatial_velocity_command[2])
#q = quaternion_from_euler(-euler[0], -euler[1], -euler[2])
# first element is w
q_orig2 = Quaternion(matrix=transform_0T.R)
#print q_orig2
# convert q_orig2 into the form that w is the last element
# inverse the quaternion by inverting w
q_orig3 = np.array(
[q_orig2[1], q_orig2[2], q_orig2[3], -q_orig2[0]])
#q_orig = np.array([0, 1, 0, 0])
#q_orig5 = quaternion_multiply(q_orig3, q_orig4)
# create a frame for velocity
br.sendTransform((0.0, 0.0, -0.2), q_orig3, rospy.Time.now(), "v",
"gripper")
#br.sendTransform((0.0, 0.2, 0.0),(0,0,0,1),rospy.Time.now(),"v_a","v_d")
msg_v.header.seq = 1
msg_v.header.stamp = rospy.Time.now()
msg_v.header.frame_id = "v"
msg_v.twist.linear.x = v_l[0] # + p[0]
msg_v.twist.linear.y = v_l[1] # + p[1]
msg_v.twist.linear.z = v_l[2] # + p[2]
# the last one should be w
#q_orig = quaternion_multiply(q_orig1, q_orig5)
msg_v.twist.angular.x = 0 #q_orig[0]
msg_v.twist.angular.y = 0 #q_orig[1]
msg_v.twist.angular.z = 0 #q_orig[2]
pub_v.publish(msg_v)
##############################################
##############################################
# compute joint velocity by QP
if self.safe_mode == True:
joints_vel = self.compute_joint_vel_cmd_qp(
joint_position, spatial_velocity_command)
joints_vel = np.clip(joints_vel, -self._robot.joint_vel_limit,
self._robot.joint_vel_limit)
# compute joint velocity by jacobian inverse
else:
joints_vel = self.compute_joint_vel_cmd_jacobian_inverse(
joint_position, spatial_velocity_command)
joints_vel = np.clip(joints_vel, -self._robot.joint_vel_limit,
self._robot.joint_vel_limit)
self._F_old = self._F_new
self._toc = self._tic
return joints_vel
vel_ctrl.set_velocity_command(np.array(qdot))
cognex_wire=detection_wire.TryGetInValue()
if cognex_wire[1][key].detected==True and cognex_wire[0] and cognex_wire[2]!=timestamp:
timestamp=cognex_wire[2]
joint_angles.append(state_w.InValue.joint_position)
cam_coordinates.append([detection_wire.InValue[key].x,detection_wire.InValue[key].y])
vel_ctrl.set_velocity_command(np.zeros((num_joints,)))
vel_ctrl.disable_velocity_mode()
#process data
eef=[]
num_samples=len(cam_coordinates)
print("num samples: ",num_samples)
for i in range(num_samples):
if robot_name=='ur':
joint_angles[i][0]+=np.pi
transform=fwdkin(robot_def,joint_angles[i])
p=transform.p
eef.append(p.tolist()[:2])
H=calibrate(cam_coordinates, eef)
H[2][-1]=robot_yaml['height']
print(H)
dict_file={'H':H.tolist()}
# directory='/home/rpi/RR_Project/calibration'
# os.chdir(directory)
with open('/home/rpi/RR_Project/calibration/'+robot_name+'.yaml', 'w') as file:
yaml.dump(dict_file, file)
def _do_grab_place_object_planar(self, robot_local_device_name,
tool_local_device_name,
robot_origin_calib_global_name,
reference_pose_global_name, object_x,
object_y, object_theta, z_offset_before,
z_offset_grab, speed_perc, grab):
var_storage = self.device_manager.get_device_client(
"variable_storage", 0.1)
robot = self.device_manager.get_device_client(robot_local_device_name)
tool = self.device_manager.get_device_client(tool_local_device_name)
reference_pose = var_storage.getf_variable_value(
"globals", reference_pose_global_name)
robot_origin_calib = var_storage.getf_variable_value(
"globals", robot_origin_calib_global_name)
T_robot = self._geom_util.named_pose_to_rox_transform(
robot_origin_calib.data.pose)
robot_info = robot.robot_info
rox_robot = self._robot_util.robot_info_to_rox_robot(robot_info, 0)
# The object's pose in world coordinates
T_object = rox.Transform(rox.rot([0., 0., 1.], object_theta),
[object_x, object_y, 0.])
# The robot's reference pose in its own frame
T_reference_pose_r = rox.fwdkin(rox_robot,
np.deg2rad(reference_pose.data))
# The robot's reference pose in world coordinates
T_reference_pose = T_robot * T_reference_pose_r
# The nominal grab pose copy
T_grab_pose_n = copy.deepcopy(T_reference_pose)
# Translate the reference pose in x and y to the nominal grab point
T_grab_pose_n.p[0] = T_object.p[0]
T_grab_pose_n.p[1] = T_object.p[1]
# Rotate the reference pose to align with the object
T_grab_pose_n.R = T_object.R @ T_grab_pose_n.R
# Before pose
T_grab_pose_before = copy.deepcopy(T_grab_pose_n)
T_grab_pose_before.p[2] += z_offset_before
# Transform to robot frame
T_grab_pose_before_r = T_robot.inv() * T_grab_pose_before
# Grab pose
T_grab_pose = copy.deepcopy(T_grab_pose_n)
T_grab_pose.p[2] += z_offset_grab
# Transform to robot frame
T_grab_pose_r = T_robot.inv() * T_grab_pose
robot_state, _ = robot.robot_state.PeekInValue()
q_current = robot_state.joint_position
## Compute inverse kinematics
# q_grab_before, res = invkin.update_ik_info3(rox_robot, T_grab_pose_before_r, q_current)
# assert res, "Invalid setpoint: invkin did not converge"
# q_grab, res = invkin.update_ik_info3(rox_robot, T_grab_pose_r, q_current)
# assert res, "Invalid setpoint: invkin did not converge"
# print(f"q_grab_before: {q_grab_before}")
# print(f"q_grab: {q_grab}")
# print()
final_seed = np.deg2rad(reference_pose.data)
traj_before = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_current, T_grab_pose_before_r, speed_perc,
final_seed)
q_init_grab = traj_before.waypoints[-1].joint_position
traj_grab = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_init_grab, T_grab_pose_r, speed_perc,
q_init_grab)
q_init_after = traj_grab.waypoints[-1].joint_position
traj_after = self._generate_movel_trajectory_tool_j_range(
robot, rox_robot, q_init_after, T_grab_pose_before_r, speed_perc,
q_init_grab)
gen = PickPlaceMotionGenerator(robot, rox_robot, tool, traj_before,
traj_grab, traj_after, grab, self._node)
# robot.jog_freespace(q_grab_before,max_velocity,True)
# time.sleep(0.5)
# robot.jog_freespace(q_grab,max_velocity*.25,True)
# time.sleep(0.5)
# robot.jog_freespace(q_grab_before,max_velocity*.25,True)
return gen
def _compute_joint_command(self, controller_state=None):
if controller_state is None:
controller_state = self._state
controller_state.controller_time = self._clock.now
step_ts = controller_state.ts * controller_state.speed_scalar
if controller_state.mode.value < 0:
if controller_state.mode == ControllerMode.ROBOT_INVALID_STATE \
or controller_state.mode == ControllerMode.ROBOT_COMMUNICATION_ERROR \
or controller_state.mode == ControllerMode.ROBOT_FAULT:
controller_state.joint_setpoint = None
controller_state.joint_command = None
controller_state.ft_wrench = None
while True:
self._update_active_trajectory(controller_state)
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(
controller_state.mode)
controller_state.active_trajectory = None
else:
break
return controller_state.mode
else:
self._update_active_trajectory(controller_state)
if controller_state.joint_setpoint is None:
controller_state.joint_setpoint = controller_state.joint_position
if controller_state.ft_wrench_threshold is not None \
and np.shape(controller_state.ft_wrench_threshold) == (6,) \
and controller_state.ft_wrench is None:
controller_state.mode = ControllerMode.SENSOR_FAULT
return ControllerMode.SENSOR_FAULT
if controller_state.joystick_command is not None \
and controller_state.joystick_command.halt_command:
controller_state.mode = ControllerMode.HALT
if controller_state.mode.value > 0 and not self._check_ft_threshold(
controller_state.ft_wrench, controller_state.ft_wrench_bias):
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(ControllerMode.FT_THRESHOLD_VIOLATION, \
"Force/Torque Threshold Violated")
controller_state.active_trajectory = None
controller_state.mode = ControllerMode.FT_THRESHOLD_VIOLATION
return ControllerMode.FT_THRESHOLD_VIOLATION
try:
if controller_state.mode == ControllerMode.HALT:
pass
elif controller_state.mode.value < ControllerMode.HALT.value:
if controller_state.active_trajectory is not None:
controller_state.active_trajectory.abort(
controller_state.mode)
controller_state.active_trajectory = None
elif controller_state.mode == ControllerMode.JOINT_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.joint_velocity_command is not None:
#controller_state.joint_setpoint += \
#controller_state.joystick_command.joint_velocity_command.dot(step_ts)
####################################################################
J = rox.robotjacobian(self._robot,
controller_state.joint_position)
cmd_vel = np.dot(
J, controller_state.joystick_command.
joint_velocity_command)
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 5)
####################################################################
#self._clip_joint_angles(controller_state)
elif controller_state.mode == ControllerMode.CARTESIAN_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, controller_state.joystick_command.spatial_velocity_command)
####################################################
########## change step_ts*5 #################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 2)
#self._clip_joint_angles(controller_state)
####################################################
elif controller_state.mode == ControllerMode.CYLINDRICAL_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
if (not all(self._robot.H[:, 0] == (0, 0, 1))
or self._robot.joint_type[0] != 0):
controller_state.mode = ControllerMode.INVALID_OPERATION
else:
cmd_vel = controller_state.joystick_command.spatial_velocity_command
transform_0T = rox.fwdkin(
self._robot, controller_state.joint_position)
d = np.linalg.norm(
(transform_0T.p[0], transform_0T.p[1]))
d = np.max((d, 0.05))
theta = np.arctan2(transform_0T.p[1],
transform_0T.p[0])
s_0 = transform_0T.p[1] / d
c_0 = transform_0T.p[0] / d
v_x = -d * s_0 * cmd_vel[3] + c_0 * cmd_vel[4]
v_y = d * c_0 * cmd_vel[3] + s_0 * cmd_vel[4]
v_z = cmd_vel[5]
w_x = cmd_vel[0]
w_y = cmd_vel[1]
w_z = cmd_vel[2] + cmd_vel[3]
cmd_vel2 = np.array([w_x, w_y, w_z, v_x, v_y, v_z])
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, cmd_vel2)
########## change step_ts*5 #################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts * 5)
####################################################
elif controller_state.mode == ControllerMode.SPHERICAL_TELEOP:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.spatial_velocity_command is not None:
if (not all(self._robot.H[:, 0] == (0, 0, 1))
or self._robot.joint_type[0] != 0):
controller_state.mode = ControllerMode.INVALID_OPERATION
else:
#TODO: more clever solution that doesn't require trigonometry?
cmd_vel = controller_state.joystick_command.spatial_velocity_command
transform_0T = rox.fwdkin(
self._robot, controller_state.joint_position)
d = np.linalg.norm(transform_0T.p)
d = np.max((d, 0.05))
theta_phi_res = rox.subproblem2(
np.dot([1, 0, 0], d), transform_0T.p, [0, 0, 1],
[0, 1, 0])
if (len(theta_phi_res) == 0):
controller_state.mode = ControllerMode.ROBOT_SINGULARITY
else:
theta_dot = cmd_vel[3]
phi_dot = cmd_vel[4]
d_dot = cmd_vel[5]
if (len(theta_phi_res) == 1):
theta, phi = theta_phi_res[0]
elif (np.abs(theta_phi_res[0][1]) <
np.deg2rad(90)):
theta, phi = theta_phi_res[0]
else:
theta, phi = theta_phi_res[1]
s_theta = np.sin(theta)
c_theta = np.cos(theta)
s_phi = np.sin(phi)
c_phi = np.cos(phi)
v_x = -d * s_phi * c_theta * phi_dot - d * s_theta * c_theta * theta_dot + c_phi * c_theta * d_dot
v_y = -d * s_phi * s_theta * phi_dot + d * c_phi * c_theta * theta_dot + s_theta * c_phi * d_dot
v_z = -d * c_phi * phi_dot - s_phi * d_dot
w_x = cmd_vel[0] - phi_dot * s_theta
w_y = cmd_vel[1] + phi_dot * c_theta
w_z = cmd_vel[2] + theta_dot
cmd_vel2 = np.array([w_x, w_y, w_z, v_x, v_y, v_z])
#self._compute_joint_vel_from_spatial_vel(controller_state, step_ts, cmd_vel2)
########## change step_ts*5 ###################
controller_state.joint_setpoint += self._quadprog.compute_quadprog(
controller_state.joint_position).dot(step_ts *
5)
###############################################
elif controller_state.mode == ControllerMode.SHARED_TRAJECTORY:
if controller_state.joystick_command is not None \
and controller_state.joystick_command.trajectory_velocity_command is not None:
active_trajectory = controller_state.active_trajectory
if active_trajectory is not None and active_trajectory.trajectory_valid:
res, setpoint = active_trajectory.increment_trajectory_time(
step_ts * controller_state.joystick_command.
trajectory_velocity_command, controller_state)
if res == ControllerMode.SUCCESS:
controller_state.joint_setpoint = setpoint
elif controller_state.mode == ControllerMode.AUTO_TRAJECTORY:
active_trajectory = controller_state.active_trajectory
if active_trajectory is not None and active_trajectory.trajectory_valid:
res, setpoint = active_trajectory.increment_trajectory_time(
step_ts, controller_state)
if res == ControllerMode.SUCCESS:
controller_state.joint_setpoint = setpoint
elif controller_state.mode.value >= ControllerMode.EXTERNAL_SETPOINT_0.value \
and controller_state.mode.value <= ControllerMode.EXTERNAL_SETPOINT_7.value:
i = controller_state.mode.value - ControllerMode.EXTERNAL_SETPOINT_0.value
setpoint = controller_state.external_joint_setpoints[i]
if setpoint is not None:
controller_state.joint_setpoint = np.copy(setpoint)
self._clip_joint_angles(controller_state)
else:
self._compute_setpoint_custom_mode(controller_state)
except:
traceback.print_exc()
controller_state.mode = ControllerMode.INTERNAL_ERROR
controller_state.joint_command = controller_state.joint_position
#TODO: add in joint command filter
controller_state.joint_command = controller_state.joint_setpoint
return controller_state.mode
def update_ik_info(R_d, p_d):
# R_d, p_d: Desired orientation and position
global robot
global d_q # Get Current Joint angles in radian ndarray
global num_joints
q_cur = d_q # initial guess on the current joint angles
q_cur = q_cur.reshape((num_joints, 1))
epsilon = 0.001 # Damping Constant
Kq = epsilon * np.eye(
len(q_cur)
) # small value to make sure positive definite used in Damped Least Square
# print_div( "<br> Kq " + str(Kq) ) # DEBUG
max_steps = 200 # number of steps to for convergence
# print_div( "<br> q_cur " + str(q_cur) ) # DEBUG
itr = 0 # Iterations
converged = False
while itr < max_steps and not converged:
pose = rox.fwdkin(robot, q_cur)
R_cur = pose.R
p_cur = pose.p
#calculate current Jacobian
J0T = rox.robotjacobian(robot, q_cur)
# Transform Jacobian to End effector frame from the base frame
Tr = np.zeros((6, 6))
Tr[:3, :3] = R_cur.T
Tr[3:, 3:] = R_cur.T
J0T = Tr @ J0T
# print_div( "<br> J0T " + str(J0T) ) # DEBUG
# Error in position and orientation
# ER = np.matmul(R_cur, np.transpose(R_d))
ER = np.matmul(np.transpose(R_d), R_cur)
#print_div( "<br> ER " + str(ER) ) # DEBUG
EP = R_cur.T @ (p_cur - p_d)
#print_div( "<br> EP " + str(EP) ) # DEBUG
#decompose ER to (k,theta) pair
k, theta = rox.R2rot(ER)
# print_div( "<br> k " + str(k) ) # DEBUG
# print_div( "<br> theta " + str(theta) ) # DEBUG
## set up s for different norm for ER
# s=2*np.dot(k,np.sin(theta)) #eR1
# s = np.dot(k,np.sin(theta/2)) #eR2
s = np.sin(theta / 2) * np.array(k) #eR2
# s=2*theta*k #eR3
# s=np.dot(J_phi,phi) #eR4
# print_div( "<br> s " + str(s) ) # DEBUG
alpha = 1 # Step size
# Damped Least square for iterative incerse kinematics
delta = alpha * (np.linalg.inv(Kq + J0T.T @ J0T) @ J0T.T @ np.hstack(
(s, EP)).T)
# print_div( "<br> delta " + str(delta) ) # DEBUG
q_cur = q_cur - delta.reshape((num_joints, 1))
# Convergence Check
converged = (np.hstack((s, EP)) < 0.0001).all()
# print_div( "<br> converged? " + str(converged) ) # DEBUG
itr += 1 # Increase the iteration
joints_text = ""
for i in q_cur:
joints_text += "(%.3f, %.3f) " % (np.rad2deg(i), i)
print_div_ik_info(
str(rox.Transform(R_d, p_d)) + "<br>" + joints_text + "<br>" +
str(converged) + ", itr = " + str(itr))
return q_cur, converged
def first_half(input, num_iter):
# stop the active RAPID program
#rapid.stop()
# reset the pointer to main
#rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(5)
# start the RAPID program
#rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
############### change ################ or there will be errors that q_pre referred before assigned
#q_pre = np.deg2rad(state.joint_angles)
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.1
### drain the force sensor buffer ###
count = 0
while count < 1000:
flag, ft = netft.read_ft_streaming(.1)
#print ft[5]
count = count + 1
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
# substract the initial force for bias
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(3)
cnt = 0
step_done = False
while cnt < n:
#t_pre = time.time()
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = state.joint_angles
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
R = pose.R
flag, ft = netft.read_ft_streaming(.1)
# map torque/force from sensor frame to base frame
T_b = np.matmul(R, ft[0:3])
F_b = np.matmul(R, ft[3:None])
F = F_b[2] # - F0# first three torques and then three forces
# start motion in y direction when robot barely touches coupon
Fd0 = 50 # this should be equal to the Fdz[0]
if F < Fd0 - 0.1 and cnt == 0:
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
# will shake if gain too large, here use 0.0002
v_z = Kp * 10 * (F - Fd0) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:,
cnt] #np.array([0, 0, 0, vdx, 0, Fd])
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - spatial_velocity_command[5]
) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
#eef_pos[2, cnt] = z
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, q_new)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# emergency stop if force too large
if abs(F) > 2000:
spatial_velocity_command = np.array([0, 0, 0, 0, 0, 0])
print "force too large, stop..."
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
#q_pre = q_new
############ change here ##############
# make the time interval 0.004 s
#t_new = time.time()
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
# interpolate to filter the output
t_inter = np.arange(0, n * delta_t, delta_t)
# interpolate each row of output
for i in range(6):
y = out[i, :]
tck = interpolate.splrep(t_inter, y,
s=0.01) # s = 0 means no interpolation
ynew = interpolate.splev(t_inter, tck, der=0)
out[i, :] = ynew
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
def update_ik_info3(
robot_rox, T_desired,
q_current): # inverse kinematics that uses Least Square solver
# R_d, p_d: Desired orientation and position
R_d = T_desired.R
p_d = T_desired.p
d_q = q_current
num_joints = len(robot_rox.joint_type)
q_cur = d_q # initial guess on the current joint angles
q_cur = q_cur.reshape((num_joints, 1))
max_steps = 200 # number of steps to for convergence
# print_div( "<br> q_cur " + str(q_cur) ) # DEBUG
hist_b = []
itr = 0 # Iterations
converged = False
while itr < max_steps and not converged:
pose = rox.fwdkin(robot_rox, q_cur.flatten())
R_cur = pose.R
p_cur = pose.p
#calculate current Jacobian
J0T = rox.robotjacobian(robot_rox, q_cur.flatten())
# Transform Jacobian to End effector frame from the base frame
Tr = np.zeros((6, 6))
Tr[:3, :3] = R_cur.T
Tr[3:, 3:] = R_cur.T
J0T = Tr @ J0T
# Jp=J0T[3:,:]
# JR=J0T[:3,:] #decompose to position and orientation Jacobian
# Error in position and orientation
# ER = np.matmul(R_cur, np.transpose(R_d))
ER = np.matmul(np.transpose(R_d), R_cur)
#print_div( "<br> ER " + str(ER) ) # DEBUG
# EP = p_cur - p_d
EP = R_cur.T @ (p_cur - p_d)
#print_div( "<br> EP " + str(EP) ) # DEBUG
#decompose ER to (k,theta) pair
k, theta = rox.R2rot(ER)
# print_div( "<br> k " + str(k) ) # DEBUG
# print_div( "<br> theta " + str(theta) ) # DEBUG
## set up s for different norm for ER
# s=2*np.dot(k,np.sin(theta)) #eR1
# s = np.dot(k,np.sin(theta/2)) #eR2
s = np.sin(theta / 2) * np.array(k) #eR2
# s=2*theta*k #eR3
# s=np.dot(J_phi,phi) #eR4
# print_div( "<br> s " + str(s) ) # DEBUG
Kp = np.eye(3)
KR = np.eye(3) #gains for position and orientation error
vd = -Kp @ EP
wd = -KR @ s
b = np.concatenate([wd, vd])
np.nan_to_num(b, copy=False, nan=0.0, posinf=None, neginf=None)
# print(b)
# print(J0T)
# DEBUG --------------
hist_b.append(b)
if itr > 0:
error_cur = np.linalg.norm(hist_b[itr - 1]) - np.linalg.norm(
hist_b[itr])
#print("Error= " + str(error_cur))
# DEBUG --------------
res = lsq_linear(J0T, b)
if res.success:
qdot_star = res.x
else:
print("Any solution could not found")
qdot_star = np.finfo(float).eps * np.ones(num_joints)
# find best step size to take
# alpha=fminbound(min_alpha,0,1,args=(q_cur,qdot_star,Sawyer_def,Rd,pd,w,Kp))
alpha = 0.3 # Step size # 1.0
delta = alpha * qdot_star
# print_div( "<br> delta " + str(delta) ) # DEBUG
# Convergence Check
converged = (np.abs(np.hstack((s, EP))) < 0.0001).all()
if not converged:
# Update for next iteration
q_cur = q_cur + delta.reshape((num_joints, 1))
# Normalize angles betweeen -pi to pi
q_cur = normalizeAngles(q_cur)
# print_div( "<br> converged? " + str(converged) ) # DEBUG
# print( "converged? " + str(converged) ) # DEBUG
itr += 1 # Increase the iteration
#print(itr)
#print(converged)
# print(delta)
# print(q_cur)
# joints_text=""
# for i in q_cur:
# joints_text+= "(%.3f, %.3f) " % (np.rad2deg(i), i)
# print_div_ik_info(str(rox.Transform(R_d,p_d)) +"<br>"+ joints_text +"<br>"+ str(converged) + ", itr = " + str(itr))
return np.squeeze(q_cur), converged
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)
def get_current_pose(self):
d_q = self.get_current_joint_positions()
pose = rox.fwdkin(
self.robot_rox,
d_q) # Returns as pose.R and pose.p, look rox for details
return pose
def main():
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
Fd = -1000
# desired velocity in x
vdx = 1
# spring constant
k = 50000
# position of object in z
pos_obj = 1.5
# feedback gain
Kp = 0.005
Kd = 0.2
Ki = 0.001
# time step
delta_t = 0.004 * 4
# initial configuration in degree
init = [0, 0, 0, 0, 90, 0]
# quadprog to solve for joint velocity
quadprog = qp.QuadProg(abb_robot)
# determine if robot has reached the initial configuration init
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(state.joint_angles) - sum(b))
if np.fabs(sum(state.joint_angles) - sum(init)) < 1e-2:
tag = False
time.sleep(0.5)
print '--------start force control--------'
pose = rox.fwdkin(abb_robot, np.deg2rad(state.joint_angles))
while pose.p[0] < 2:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, np.deg2rad(state.joint_angles))
if pose.p[2] >= pos_obj:
F = 0
v_l = np.array([0, 0, -Kp * (F - Fd)])
else:
F = k * (pose.p[2] - pos_obj)
v_l = np.array([vdx, 0, -Kp * (F - Fd)])
print F
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, v_l[0], v_l[1], v_l[2]])
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, np.deg2rad(state.joint_angles))
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
np.deg2rad(state.joint_angles), spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = np.deg2rad(state.joint_angles) + joints_vel * delta_t
egm.send_to_robot(q_c)
def calibrate(images, joint_poses, aruco_dict, aruco_id, aruco_markersize,
flange_to_marker, mtx, dist, cam_pose, rox_robot,
robot_local_device_name):
""" Apply extrinsic camera calibration operation for images in the given directory path
using opencv aruco marker detection, the extrinsic marker poses given in a json file,
and the given intrinsic camera parameters."""
assert aruco_dict.startswith("DICT_"), "Invalid aruco dictionary name"
aruco_dict = getattr(aruco, aruco_dict) # convert string to python
aruco_dict = cv2.aruco.Dictionary_get(aruco_dict)
aruco_params = cv2.aruco.DetectorParameters_create()
i = 0
imgs_out = []
geom_util = GeometryUtil()
image_util = ImageUtil()
object_points = []
image_points = []
for img, joints in zip(images, joint_poses):
# Find the aruco tag corners
# corners, ids, rejected = cv2.aruco.detectMarkers(img, aruco_dict, parameters=aruco_params,cameraMatrix=mtx, distCoeff=dist)
corners, ids, rejected = cv2.aruco.detectMarkers(
img, aruco_dict, parameters=aruco_params)
# #debug
# print(str(type(corners))) # <class 'list'>
# print(str(corners)) # list of numpy arrays of corners
# print(str(type(ids))) # <class 'numpy.ndarray'>
# print(str(ids))
if len(corners) > 0:
# Only find the id that we desire
indexes = np.flatnonzero(ids.flatten() == aruco_id).tolist()
corners = [corners[index] for index in indexes]
ids = np.asarray([ids[index] for index in indexes])
# #debug
# print(str(type(corners))) # <class 'list'>
# print(str(corners)) # list of numpy arrays of corners
# print(str(type(ids))) # <class 'numpy.ndarray'>
# print(str(ids))
if len(ids) > 0: # if there exist at least one id that we desire
# Select the first detected one, discard the others
corners = corners[0] # now corners is 1 by 4
# # extract the marker corners (which are always returned
# # in top-left, top-right, bottom-right, and bottom-left
# # order)
# corners = corners.reshape((4, 2))
# (topLeft, topRight, bottomRight, bottomLeft) = corners
# Estimate the pose of the detected marker in camera frame
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(
corners, aruco_markersize, mtx, dist)
# # Debug: Show the detected tag and axis in the image
# # # cv2.aruco.drawDetectedMarkers(img, corners) # Draw A square around the markers (Does not work)
img1 = img.copy()
img_out = cv2.aruco.drawAxis(img1, mtx, dist, rvec, tvec,
aruco_markersize *
0.75) # Draw Axis
imgs_out.append(img_out)
transform_base_2_flange = rox.fwdkin(rox_robot, joints)
transform_flange_2_marker = geom_util.pose_to_rox_transform(
flange_to_marker)
transform_base_2_marker = transform_base_2_flange * transform_flange_2_marker
transform_base_2_marker_corners = _marker_corner_poses(
transform_base_2_marker, aruco_markersize)
# Structure of this disctionary is "filename":[[R_base2marker],[T_base2marker],[R_cam2marker],[T_cam2marker]]
for j in range(4):
object_points.append(transform_base_2_marker_corners[j].p)
image_points.append(corners[0, j])
#pose_pairs_dict[i] = (transform_base_2_marker_corners, corners)
i += 1
object_points_np = np.array(object_points, dtype=np.float64)
image_points_np = np.array(image_points, dtype=np.float32)
# Finally execute the calibration
retval, rvec, tvec = cv2.solvePnP(object_points_np, image_points_np, mtx,
dist)
R_cam2base = cv2.Rodrigues(rvec)[0]
T_cam2base = tvec
# Add another display image of marker at robot base
img_out = cv2.aruco.drawAxis(img, mtx, dist,
cv2.Rodrigues(R_cam2base)[0], T_cam2base,
aruco_markersize * 0.75) # Draw Axis
imgs_out.append(img_out)
rox_transform_cam2base = rox.Transform(R_cam2base, T_cam2base,
cam_pose.parent_frame_id,
robot_local_device_name)
rox_transform_world2base = cam_pose * rox_transform_cam2base
#R_base2cam = R_cam2base.T
#T_base2cam = - R_base2cam @ T_cam2base
R_base2cam = rox_transform_world2base.inv().R
T_base2cam = rox_transform_world2base.inv().p
#debug
print("FINAL RESULTS: ")
print("str(R_cam2base)")
# print(str(type(R_cam2base)))
print(str(R_cam2base))
print("str(T_cam2base)")
# print(str(type(T_cam2base.flatten())))
print(str(T_cam2base))
print("str(R_base2cam)")
# print(str(type(R_base2cam)))
print(str(R_base2cam))
print("str(T_base2cam)")
# print(str(type(T_base2cam.flatten())))
print(str(T_base2cam))
pose_res = geom_util.rox_transform_to_named_pose(rox_transform_world2base)
cov = np.eye(6) * 1e-5
imgs_out2 = [
image_util.array_to_compressed_image_jpg(i, 70) for i in imgs_out
]
return pose_res, cov, imgs_out2, 0.0
def main():
# initialize EGM interface instance
egm = rpi_abb_irc5.EGM()
# initialize a robot instance
abb_robot = abb_irb6640_180_255_robot()
# desired force
#Fd = 1000
# desired velocity in y
#vdy = -0.5
# feedback gain
Kp = 0.0002
Kd = 0.0008
Ki = 0.0004
# time step
delta_t = 0.004
# initial configuration in degree
init = [-91.85, 2.53, 38.20, 0.00, 49.27, -1.85]
n = 2000
# quadprog to solve for joint velocity
quadprog = qp.QuadProg(abb_robot)
# force sensor interface
try:
if (len(sys.argv) < 2):
raise Exception('IP address of ATI Net F/T sensor required')
host = sys.argv[1]
netft = rpi_ati_net_ft.NET_FT(host)
netft.set_tare_from_ft()
netft.start_streaming()
except KeyboardInterrupt:
pass
####### trapezoidal desired force in z #######
tt = np.linspace(0, 4 * np.pi, n)
desired_f = np.zeros((1, n))
vdy = np.zeros((1, n))
# form trap force and trap motion
for i in range(n):
if tt[i] >= 0 and tt[i] < np.pi:
desired_f[0, i] = 50 + 302 * tt[i]
vdy[0, i] = -0.2 * tt[i]
elif tt[i] >= np.pi and tt[i] < 3 * np.pi:
desired_f[0, i] = 50 + 302 * np.pi
vdy[0, i] = -0.2 * np.pi
else:
desired_f[0, i] = 50 + 302 * np.pi - 302 * (tt[i] - 3 * np.pi)
vdy[0, i] = -0.2 * np.pi + 0.2 * (tt[i] - 3 * np.pi)
#plt.plot(vdy[0, :])
#plt.show()
######## change here ########
#pos = 0
#acce = 0
#v_l_pre = 0
#########
# output force
force_out = np.zeros((6, n))
# pos of eef
eef_pos = np.zeros((3, n))
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
# referece height of coupon that achieves desired force
z_ref = 0.89226
x_ref = 0
y_ref = -1.35626
# determine if robot has reached the initial configuration init
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print sum(np.rad2deg(state.joint_angles))- sum(init)
if np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init)) < 1e-3:
tag = False
time.sleep(1)
print '--------start force control--------'
### drain the force sensor buffer ###
for i in range(1000):
flag, ft = netft.read_ft_streaming(.1)
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
flag, ft = netft.read_ft_streaming(.1)
F0 = ft[5]
print F0
time.sleep(2)
cnt = 0
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
# forward kinematics
pose = rox.fwdkin(abb_robot, state.joint_angles)
R = pose.R
flag, ft = netft.read_ft_streaming(.1)
# map torque/force from sensor frame to base frame
T_b = np.matmul(R, ft[0:3])
F_b = np.matmul(R, ft[3:None])
F = F_b[2] # first three torques and then three forces
Fd0 = 50
# do not start motion in y until robot barely touches coupon (50 N)
if F < Fd0 - 0.1 and cnt == 0:
z = pose.p[2]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
# will shake if gain too large, here use 0.0002
v_z = Kp * 10 * (F - Fd0)
v_l = np.array([0, 0, v_z])
else:
# deadzone for Fx
if abs(F_b[0]) < 30:
F_x = 0
# deadzone for Fy
if abs(F_b[1]) < 30:
F_y = 0
v_x = Kp / 2 * (F_x - 0)
v_y = vdy[0, cnt] + Kp / 2 * (F_y - 0)
z = pose.p[2]
#print desired_f[0, cnt]
# account for the robot base and length of tool
z = z + 0.026 - 0.18 + 0.00353
v_z = Kp * (F - desired_f[0, cnt])
v_l = np.array([v_x, v_y, v_z])
force_out[:, cnt] = np.concatenate((T_b, F_b), axis=0)
eef_pos[:, cnt] = pose.p
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt += 1
print F
#### change here ####
#pos = pos + v_l[2]*delta_t
#acce = (v_l[2]-v_l_pre)/delta_t
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, v_l[0], v_l[1], v_l[2]])
# emergency stop if force too large
if abs(F) > 2000:
spatial_velocity_command = np.array([0, 0, 0, 0, 0, 0])
print "force too large, stop..."
# solve for joint velocity
# Jacobian inverse
#J = rox.robotjacobian(abb_robot, state.joint_angles)
#joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
joints_vel = quadprog.compute_joint_vel_cmd_qp(
state.joint_angles, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = state.joint_angles + joints_vel * delta_t
egm.send_to_robot(q_c)
####### change here ########
#v_l_pre = v_l[2]
#if t_new - t_pre < delta_t:
# time.sleep(delta_t - t_new + t_pre)
if cnt == n:
csv_dat = np.hstack((desired_f.T, vdy.T, force_out.T, eef_pos.T,
eef_orien.T, tim.T))
np.savetxt(
'trap_force_trap_motion_020520.csv',
csv_dat,
fmt='%6.5f',
delimiter=',') #, header='desired joint, optimal input')
print "done"
def first_half(input, num_iter):
# stop the active RAPID program
rapid.stop()
# reset the pointer to main
rapid.resetpp()
print 'first half'
print 'reset PP to main'
time.sleep(2)
# start the RAPID program
rapid.start()
# determine if robot has reached the initial configuration
tag = True
while tag:
res, state = egm.receive_from_robot(.1)
if res:
#print np.fabs(sum(np.rad2deg(state.joint_angles)) - sum(init))
if np.fabs(sum(state.joint_angles) - sum(init)) < 1e-4:
tag = False
time.sleep(1)
# out is composed of 5 velocity and 1 force in z
out = np.zeros((6, n))
force_out = np.zeros((1, n))
# pos of eef
eef_pos = np.zeros((3, n))
# orientation of eef (quaternion)
eef_orien = np.zeros((4, n))
# timestamp
tim = np.zeros((1, n))
q_hat = np.zeros((6, 1))
qhat_dot = np.zeros((6, 1))
# for observer k should be symmetric and positive definite
kl = 0.5
### drain the EGM buffer ###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
time.sleep(2)
cnt = 0
step_done = False
while cnt < n:
# receive EGM feedback
res, state = egm.receive_from_robot(.1)
if not res:
continue
q_new = np.deg2rad(state.joint_angles)
# step-over
if not step_done:
print '--------start step-over motion--------'
# do step-over of 0.25 mm in +x in world frame
# current eef pose
pose_cur = rox.fwdkin(abb_robot, q_new)
pose_cur.p[0] = pose_cur.p[0] + num_iter * 2 * step_over
# solve for inverse kinematics and pick the one that is closest to current configuration
sol = rox.robot6_sphericalwrist_invkin(abb_robot, pose_cur, q_new)
try:
tar = sol[0] # raise exception if no solution
except:
tar = q_new
# move to the new position after step-over
egm.send_to_robot(tar)
step_done = True
q_new = tar
### drain the EGM buffer, or it will use the obsolete EGM feedback###
for i in range(1000):
res, state = egm.receive_from_robot(.1)
print '--------step-over motion done--------'
time.sleep(2)
# forward kinematics to calculate current position of eef
pose = rox.fwdkin(abb_robot, q_new)
R = pose.R
Fd0 = 50
if pose.p[2] >= pos_obj:
F = 0
v_z = Kp * 30 * (F - Fd0) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
F = -k * (pose.p[2] - pos_obj)
if F < Fdz - 0.5 and cnt == 0:
v_z = Kp * (F - Fdz) #-Ki*(z-z_ref)-Kd*acce#-Ki*pos
# formalize entire twist
spatial_velocity_command = np.array([0, 0, 0, 0, 0, v_z])
else:
# formalize entire twist
# nominal input composed of F and v
spatial_velocity_command = input[:, cnt]
v_z = Kp * (F - spatial_velocity_command[5])
# nominal input only contains v
spatial_velocity_command[5] = v_z
# approximation of joint velocity
#q_new = np.deg2rad(state.joint_angles)
######### change here, use observer instead of approximation to calculate q_dot ########
if cnt == 0:
q_hat = q_new
qhat_dot = joints_vel + kl * (q_new - q_hat)
#q_dot = (q_new - q_pre)/delta_t
J = rox.robotjacobian(abb_robot, q_new)
# estimate velocity
v_est = J.dot(qhat_dot)
# formalize the nominal output composed of F and v
out[:, cnt] = np.append(v_est[0:5], F)
force_out[:, cnt] = F
eef_pos[:, cnt] = pose.p
R = pose.R
quat = rox.R2q(R)
eef_orien[:, cnt] = quat
tim[0, cnt] = time.time()
cnt = cnt + 1
print F
# solve for joint velocity
# Jacobian inverse
J = rox.robotjacobian(abb_robot, q_new)
joints_vel = np.linalg.pinv(J).dot(spatial_velocity_command)
# QP
#joints_vel = quadprog.compute_joint_vel_cmd_qp(q_new, spatial_velocity_command)
# commanded joint position setpoint to EGM
q_c = q_new + joints_vel * delta_t
egm.send_to_robot(q_c)
# joint angle at previous time step
######### change here ########
q_hat = q_hat + qhat_dot * delta_t
error = out - desired
# flip the error
err_flip = np.fliplr(error)
print np.linalg.norm(error, 'fro')
return out, err_flip, np.linalg.norm(
error, 'fro'), force_out, eef_pos, eef_orien, tim
def compute_fk(self, joint = None):
if joint is None:
joint=self.moveit_group.get_current_joint_values()
return rox.fwdkin(self.rox_robot, joint)
def _generate_movel_trajectory(self, robot_client, rox_robot,
initial_q_or_T, T_final, speed_perc,
q_final_seed):
JointTrajectoryWaypoint = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectoryWaypoint",
robot_client)
JointTrajectory = RRN.GetStructureType(
"com.robotraconteur.robotics.trajectory.JointTrajectory",
robot_client)
dof = len(rox_robot.joint_type)
if isinstance(initial_q_or_T, rox.Robot):
T_initial = initial_q_or_T
q_initial = invkin.update_ik_info3(rox_robot, initial_q_or_T,
np.random.randn((dof, )))
else:
q_initial = initial_q_or_T
T_initial = rox.fwdkin(rox_robot, q_initial)
p_diff = T_final.p - T_initial.p
R_diff = np.transpose(T_initial.R) @ T_final.R
k, theta = rox.R2rot(R_diff)
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.25 * 180 / np.pi
N_samples_translate = np.linalg.norm(p_diff) * 100.0
N_samples_rot = np.abs(theta) * 0.2 * 180 / np.pi
N_samples = math.ceil(np.max((N_samples_translate, N_samples_rot, 20)))
ss = np.linspace(0, 1, N_samples)
if q_final_seed is None:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_initial)
else:
q_final, res = invkin.update_ik_info3(rox_robot, T_final,
q_final_seed)
#assert res, "Inverse kinematics failed"
way_pts = np.zeros((N_samples, dof), dtype=np.float64)
way_pts[0, :] = q_initial
for i in range(1, N_samples):
s = float(i) / (N_samples - 1)
R_i = T_initial.R @ rox.rot(k, theta * s)
p_i = (p_diff * s) + T_initial.p
T_i = rox.Transform(R_i, p_i)
#try:
# q, res = invkin.update_ik_info3(rox_robot, T_i, way_pts[i-1,:])
#except AssertionError:
ik_seed = (1.0 - s) * q_initial + s * q_final
q, res = invkin.update_ik_info3(rox_robot, T_i, ik_seed)
assert res, "Inverse kinematics failed"
way_pts[i, :] = q
vlims = rox_robot.joint_vel_limit
alims = rox_robot.joint_acc_limit
path = ta.SplineInterpolator(ss, way_pts)
pc_vel = constraint.JointVelocityConstraint(vlims * 0.95 * speed_perc /
100.0)
pc_acc = constraint.JointAccelerationConstraint(alims)
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
parametrizer="ParametrizeConstAccel")
jnt_traj = instance.compute_trajectory()
if jnt_traj is None:
return None
ts_sample = np.linspace(0, jnt_traj.duration, N_samples)
qs_sample = jnt_traj(ts_sample)
waypoints = []
for i in range(N_samples):
wp = JointTrajectoryWaypoint()
wp.joint_position = qs_sample[i, :]
wp.time_from_start = ts_sample[i]
waypoints.append(wp)
traj = JointTrajectory()
traj.joint_names = rox_robot.joint_names
traj.waypoints = waypoints
return traj
