def pbvs(self, kp, tols_p, tols_r, abort_force, max_iters = 25, no_z = False, z_offset = 0): i=0 while True: if i > max_iters: raise Exception("Placement controller timeout") if self._aborted: raise Exception("Operation aborted") img = self.receive_image() target_pose, err = self.compute_step_gripper_target_pose(img, kp, no_z = no_z, z_offset = z_offset) err_p = np.linalg.norm(err.p) if no_z: err_p = np.linalg.norm(err.p[0:2]) err_r = np.abs(rox.R2rot(err.R)[1]) if err_p < tols_p and err_r < tols_r: return err_p, err_r #target_pose.p[1]-=0.01 # Offset a little to avoid panel overlap plan = self.planner.trajopt_plan(target_pose, json_config_name = "panel_placement") if self._aborted: raise Exception("Operation aborted") self.controller_commander.set_controller_mode(self.controller_commander.MODE_AUTO_TRAJECTORY,0.7,[], abort_force) self.controller_commander.execute_trajectory(plan) i+=1
def test_rot2q(): k, theta=rox.R2rot(np.array([[-0.5057639,-0.1340537,0.8521928], \ [0.6456962,-0.7139224,0.2709081], \ [0.5720833,0.6872731,0.4476342]])) q_t = np.array([0.2387194, 0.4360402, 0.2933459, 0.8165967]) q = rox.rot2q(k, theta) np.testing.assert_allclose(q_t, q, atol=1e-6)
def compute_step_gripper_target_pose(self, img, Kp, no_z=False, z_offset=0): fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform( img) if no_z: error_transform.p[2] = 0 else: error_transform.p[2] -= z_offset gripper_to_camera_tf = self.tf_listener.lookupTransform( "vacuum_gripper_tool", "gripper_camera_2", rospy.Time(0)) world_to_vacuum_gripper_tool_tf = self.tf_listener.lookupTransform( "world", "vacuum_gripper_tool", rospy.Time(0)) #Scale by Kp k, theta = rox.R2rot(error_transform.R) r = np.multiply(k * theta, Kp[0:3]) r_norm = np.linalg.norm(r) if (r_norm < 1e-6): error_transform2_R = np.eye(3) else: error_transform2_R = rox.rot(r / r_norm, r_norm) error_transform2_p = np.multiply(error_transform.p, (Kp[3:6])) error_transform2 = rox.Transform(error_transform2_R, error_transform2_p) gripper_to_fixed_marker_tf = gripper_to_camera_tf * fixed_marker_transform gripper_to_desired_fixed_marker_tf = gripper_to_fixed_marker_tf * error_transform2 #print gripper_to_fixed_marker_tf ret = world_to_vacuum_gripper_tool_tf * ( gripper_to_desired_fixed_marker_tf * gripper_to_fixed_marker_tf.inv()).inv() #print world_to_vacuum_gripper_tool_tf #print ret #print error_transform return ret, error_transform
def _R2rot_test(k1, theta1): R = rox.rot(k1, theta1) k2, theta2 = rox.R2rot(R) if abs(theta1 - theta2) > (theta1 + theta2): k2 = -k2 theta2 = -theta2 np.testing.assert_allclose(theta1, theta2, atol=1e-6) if (abs(theta1) < 1e-9): return if ((np.abs(theta1) - np.pi) < 1e-9): if np.linalg.norm(k1 + k2) < 1e-6: np.testing.assert_allclose(k1, -k2, atol=1e-6) return np.testing.assert_allclose(k1, k2, atol=1e-6) return np.testing.assert_allclose(k1, k2, atol=1e-6)
def compute_step_gripper_target_pose(img, fixed_marker, payload_marker, desired_transform, \ camera_info, aruco_dict, Kp, tf_listener): fixed_marker_transform, error_transform = compute_error_transform(img, fixed_marker, payload_marker, \ desired_transform, camera_info, aruco_dict) gripper_to_camera_tf = tf_listener.lookupTransform("vacuum_gripper_tool", "gripper_camera_2", rospy.Time(0)) world_to_vacuum_gripper_tool_tf = tf_listener.lookupTransform( "world", "vacuum_gripper_tool", rospy.Time(0)) #Scale by Kp k, theta = rox.R2rot(error_transform.R) r = np.multiply(k * theta, Kp[0:3]) r_norm = np.linalg.norm(r) if (r_norm < 1e-6): error_transform2_R = np.eye(3) else: error_transform2_R = rox.rot(r / r_norm, r_norm) error_transform2_p = np.multiply(error_transform.p, (Kp[3:6])) error_transform2 = rox.Transform(error_transform2_R, error_transform2_p) gripper_to_fixed_marker_tf = gripper_to_camera_tf * fixed_marker_transform gripper_to_desired_fixed_marker_tf = gripper_to_fixed_marker_tf * error_transform2 #print gripper_to_fixed_marker_tf ret = world_to_vacuum_gripper_tool_tf * ( gripper_to_desired_fixed_marker_tf * gripper_to_fixed_marker_tf.inv()).inv() #print world_to_vacuum_gripper_tool_tf #print ret print error_transform return ret
def pbvs_jacobian(self): self.controller_commander.set_controller_mode( self.controller_commander.MODE_AUTO_TRAJECTORY, 0.7, [], []) tvec_err = [100, 100, 100] rvec_err = [100, 100, 100] self.FTdata_0 = self.FTdata error_transform = rox.Transform(rox.rpy2R([2, 2, 2]), np.array([100, 100, 100])) FT_data_ori = [] FT_data_biased = [] err_data_p = [] err_data_rpy = [] joint_data = [] time_data = [] #TODO: should listen to stage_3_tol_r not 1 degree print self.desired_camera2_transform #R_desired_cam = self.desired_camera2_transform.R.dot(self.desired_transform.R) #R_desired_cam = R_desired_cam.dot(self.desired_camera2_transform.R.transpose()) #t_desired_cam = -self.desired_camera2_transform.R.dot(self.desired_transform.p) while (error_transform.p[2] > 0.01 or np.linalg.norm( [error_transform.p[0], error_transform.p[1]]) > self.stage3_tol_p or np.linalg.norm(rox.R2rpy(error_transform.R)) > np.deg2rad(1)): img = self.receive_image() fixed_marker_transform, payload_marker_transform, error_transform = self.compute_error_transform( img) #print self.desired_transform.R.T, -fixed_marker_transform.R.dot(self.desired_transform.p) R_desired_cam = fixed_marker_transform.R.dot( self.desired_transform.R) R_desired_cam = R_desired_cam.dot( fixed_marker_transform.R.transpose()) t_desired_cam = -fixed_marker_transform.R.dot( self.desired_transform.p) # Compute error directly in the camera frame observed_R_difference = np.dot( payload_marker_transform.R, fixed_marker_transform.R.transpose()) k, theta = rox.R2rot( np.dot(observed_R_difference, R_desired_cam.transpose()) ) #np.array(rox.R2rpy(rvec_err1)) rvec_err1 = k * theta observed_tvec_difference = fixed_marker_transform.p - payload_marker_transform.p tvec_err1 = -fixed_marker_transform.R.dot( self.desired_transform.p) - observed_tvec_difference #print 'SPOT: ',tvec_err1, rvec_err1 # Map error to the robot spatial velocity world_to_camera_tf = self.tf_listener.lookupTransform( "world", "gripper_camera_2", rospy.Time(0)) camera_to_link6_tf = self.tf_listener.lookupTransform( "gripper_camera_2", "link_6", rospy.Time(0)) t21 = -np.dot( rox.hat( np.dot(world_to_camera_tf.R, (camera_to_link6_tf.p - payload_marker_transform.p.reshape((1, 3))).T)), world_to_camera_tf.R) #np.zeros((3,3))# # v = R_oc(vc)c + R_oc(omeega_c)_c x (r_pe)_o = R_oc(vc)c - (r_pe)_o x R_oc(omeega_c)_c tvec_err = t21.dot(rvec_err1).reshape( (3, )) + world_to_camera_tf.R.dot(tvec_err1).reshape((3, )) # omeega = R_oc(omeega_c)_c rvec_err = world_to_camera_tf.R.dot(rvec_err1).reshape((3, )) tvec_err = np.clip(tvec_err, -0.2, 0.2) rvec_err = np.clip(rvec_err, -np.deg2rad(5), np.deg2rad(5)) if tvec_err[2] < 0.03: rospy.loginfo("Only Compliance Control") tvec_err[2] = 0 rot_err = rox.R2rpy(error_transform.R) rospy.loginfo("tvec difference: %f, %f, %f", error_transform.p[0], error_transform.p[1], error_transform.p[2]) rospy.loginfo("rvec difference: %f, %f, %f", rot_err[0], rot_err[1], rot_err[2]) dx = -np.concatenate((rvec_err, tvec_err)) * self.K_pbvs print np.linalg.norm([error_transform.p[0], error_transform.p[1]]) print np.linalg.norm(rox.R2rpy(error_transform.R)) # Compliance Force Control if (not self.ft_flag): raise Exception("havent reached FT callback") # Compute the exteranl force FTread = self.FTdata - self.FTdata_0 # (F)-(F0) print '================ FT1 =============:', FTread print '================ FT2 =============:', self.FTdata if FTread[-1] > (self.F_d_set1 + 50): F_d = self.F_d_set1 else: F_d = self.F_d_set2 if (self.FTdata == 0).all(): rospy.loginfo("FT data overflow") dx[-1] += self.K_pbvs * 0.004 else: tx_correct = 0 if abs(self.FTdata[0]) > 80: tx_correct = 0.0002 * (abs(self.FTdata[0]) - 80) Vz = self.Kc * (F_d - FTread[-1]) + tx_correct dx[-1] = dx[-1] + Vz print "dx:", dx current_joint_angles = self.controller_commander.get_current_joint_values( ) joints_vel = QP_abbirb6640( np.array(current_joint_angles).reshape(6, 1), np.array(dx)) goal = self.trapezoid_gen( np.array(current_joint_angles) + joints_vel.dot(1), np.array(current_joint_angles), 0.008, 0.008, 0.015) #acc,dcc,vmax) print "joints_vel:", joints_vel self.client.wait_for_server() self.client.send_goal(goal) self.client.wait_for_result() res = self.client.get_result() if (res.error_code != 0): raise Exception("Trajectory execution returned error") FT_data_ori.append(self.FTdata) FT_data_biased.append(FTread) err_data_p.append(error_transform.p) err_data_rpy.append(rot_err) joint_data.append(current_joint_angles) time_data.append(time.time()) filename_pose = "/home/rpi-cats/Desktop/YC/Data/Panel2_Placement_In_Nest_Pose_" + str( time.time()) + ".mat" scipy.io.savemat(filename_pose, mdict={ 'FT_data_ori': FT_data_ori, 'FT_data_biased': FT_data_biased, 'err_data_p': err_data_p, 'err_data_rpy': err_data_rpy, 'joint_data': joint_data, 'time_data': time_data }) rospy.loginfo("End ====================")
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 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
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