def get_joint_forces(self, ptAtual, ptFinal, oriAtual, dist_EOF_to_Goal, err_ori): # Get UR5 Jacobian of each link Jacobian = get_geometric_jacobian(self.ur5_param, self.actual_position) # Getting attractive forces forces_p = np.zeros((3, 1)) forces_w = np.zeros((3, 1)) for i in range(3): if abs(ptAtual[i] - ptFinal[i]) <= self.dist_att: f_att_l = -self.zeta*(ptAtual[i] - ptFinal[i]) else: f_att_l = -self.dist_att*self.zeta*(ptAtual[i] - ptFinal[i])/dist_EOF_to_Goal[i] if abs(oriAtual[i] - self.Displacement[i]) <= self.dist_att_config: f_att_w = -self.zeta*(oriAtual[i] - self.Displacement[i]) else: f_att_w = -self.dist_att_config*self.zeta*(oriAtual[i] - self.Displacement[i])/dist_EOF_to_Goal[i] forces_p[i, 0] = f_att_l forces_w[i, 0] = f_att_w forces_p = np.asarray(forces_p) JacobianAtt_p = np.asarray(Jacobian[5]) joint_att_force_p = JacobianAtt_p.dot(forces_p) joint_att_force_p = np.multiply(joint_att_force_p, [[0.5], [0.1], [1.5], [1], [1], [1]]) forces_w = np.asarray(forces_w) JacobianAtt_w = np.asarray(Jacobian[6]) joint_att_force_w = JacobianAtt_w.dot(forces_w) joint_att_force_w = np.multiply(joint_att_force_w, [[0], [0.1], [0.1], [0.4], [0.4], [0.4]]) return np.transpose(joint_att_force_p), np.transpose(joint_att_force_w)
def main(): ur5_robot = MoveGroupPythonIntefaceTutorial() ur5_robot.delete_markers() ### UR5 Initial position raw_input("' =========== Aperte enter para posicionar o UR5 \n") ur5_robot.joint_states.position = [ 0, -1.5707, 0, -1.5707, 0, 0 ] # Posicao configurada no fake_controller_joint_states ur5_robot.pub_joint_states.publish(ur5_robot.joint_states) raw_input( "' =========== Aperte enter para carregar o obstaculo e objetivo \n") ### Final and obstacle points obs_pos = [0.45, 0.4, 0.4] diam_obs = 0.4 ur5_robot.add_sphere(obs_pos, 11, diam_obs, ColorRGBA(1.0, 0.0, 0.0, 0.5)) ptFinal = [0.45, 0.3, 0.5] oriFinal = [0.01, 0.01, 0.01] diam_goal = 0.04 ur5_robot.add_sphere(ptFinal, 13, diam_goal, ColorRGBA(0.0, 1.0, 0.0, 0.8)) ### CPA Parameters err = diam_goal / 2 max_iter = 5000 zeta = [0.5 for i in range(7)] eta = [0.005 for i in range(6)] rho_0 = diam_obs dist_att = 0.05 dist_att_config = 0.15 alfa = 0.5 alfa_rot = 0.2 diam_rep_ur5 = 0.15 raw_input("' =========== Pressione enter para posicionar o UR5 \n") ur5_robot.joint_states.position = [ 0, -1.5707, 0, -1.5707, 0, 0 ] # Posicao configurada no fake_controller_joint_states ur5_robot.pub_joint_states.publish(ur5_robot.joint_states) ### GET repulsive control point's position through LOOKUPTRANSFORM CP_pos, CP_dist = ur5_robot.get_repulsive_cp( obs_pos, ur5_robot.joint_states.position, diam_rep_ur5) ### Parameters CPAA_state = False Orientation_state = True hz = get_param("rate", 50) # 10hz r = rospy.Rate(hz) ptAtual, oriAtual = ur5_robot.tf.lookupTransform("base_link", "grasping_link", rospy.Time()) dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) n = 0 raw_input("' =========== Aperte enter para iniciar o algoritmo dos CPAs") while dist_EOF_to_Goal > err and not rospy.is_shutdown(): Jacobian = get_geometric_jacobian(ur5_robot.ur5_param, ur5_robot.joint_states.position) CP_pos, CP_dist = ur5_robot.get_repulsive_cp( obs_pos, ur5_robot.joint_states.position, diam_rep_ur5) joint_att_force_p, joint_att_force_w, joint_rep_force = CPA_classico.get_joint_forces( ptAtual, ptFinal, oriAtual, oriFinal, dist_EOF_to_Goal, Jacobian, ur5_robot.joint_states.position, ur5_robot.ur5_param, zeta, eta, rho_0, dist_att, dist_att_config, CP_dist, CP_pos, obs_pos, CPAA_state, diam_rep_ur5) # Joint angles UPDATE ur5_robot.joint_states.position = ur5_robot.joint_states.position + alfa * joint_att_force_p[ 0] if Orientation_state: ur5_robot.joint_states.position = ur5_robot.joint_states.position + alfa_rot * joint_att_force_w[ 0] list = np.transpose(joint_rep_force[0]).tolist() for j in range(6): for i in range(6): ur5_robot.joint_states.position[ i] = ur5_robot.joint_states.position[i] + alfa * list[j][i] ptAtual, oriAtual = ur5_robot.tf.lookupTransform( "base_link", "grasping_link", rospy.Time()) oriAtual += quaternion_from_euler(1.5707, 0, 0) if n % 10 == 0: ur5_robot.visualize_path_planned(ptAtual) print("Distance to the goal: " + str(dist_EOF_to_Goal)) dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) ur5_robot.pub_joint_states.publish(ur5_robot.joint_states) try: r.sleep() except rospy.exceptions.ROSTimeMovedBackwardsException: pass n += 1
def main(args): ur5_robot = MoveGroupPythonIntefaceTutorial(args) way_points = [] ur5_robot.scene.clear() # UR5 Initial position raw_input("' =========== Aperte enter para posicionar o UR5 \n") # Posicao configurada no fake_controller_joint_states ur5_robot.joint_states.position = [0, -1.5707, 0, -1.5707, 1.5707, 0] way_points.append(ur5_robot.joint_states.position) ur5_robot.move(way_points, "gazebo") # raw_input("' =========== Aperte enter para carregar os param. dos CPAs \n") # Obstacle positions oc = [-0.9, 0, 0.375] # Obstacle reference point - 3D printer d1 = -0.080 s = 1 obs_pos = [ oc, np.add(oc, [s * 0.14, 0.0925, 0.255 + d1]), np.add(oc, [s * 0.14, 0.185, 0.255 + d1]), np.add(oc, [s * 0.14, 0, 0.255 + d1]), np.add(oc, [s * 0.14, -0.0925, 0.255 + d1]), np.add(oc, [s * 0.14, -0.185, 0.255 + d1]), np.add(oc, [s * 0.14, -0.185, 0.16 + d1]), np.add(oc, [s * 0.14, 0.185, 0.16 + d1]), np.add(oc, [s * 0.14, 0.0925, 0.05 + d1]), np.add(oc, [s * 0.14, 0.185, 0.05 + d1]), np.add(oc, [s * 0.14, 0, 0.05 + d1]), np.add(oc, [s * 0.14, -0.0925, 0.05 + d1]), np.add(oc, [s * 0.14, -0.185, 0.05 + d1]) ] diam_obs = [0.18] * len(obs_pos) # Main obstacle repulsive field diam_obs[0] = 0.3 ur5_robot.add_sphere(obs_pos, diam_obs, ColorRGBA(1.0, 0.0, 0.0, 0.5)) # add_obstacles(name, height, radius, pose, orientation, r, g, b): # ur5_robot.add_obstacles("up", 0.54, 0.09, [-0.76, 0, 0.345], [1.5707, 1.5707, 0], 1, 0, 0) # ur5_robot.add_obstacles("bottom", 0.54, 0.09, [-0.76, 0, 0.55], [1.5707, 1.5707, 0], 1, 0, 0) # ur5_robot.add_obstacles("right", 0.35, 0.09, [-0.76, 0.185, 0.455], [0, 0, 0], 1, 0, 0) # ur5_robot.add_obstacles("left", 0.35, 0.09, [-0.76, -0.185, 0.455], [0, 0, 0], 1, 0, 0) # apply obstacle colors to moveit ur5_robot.scene.sendColors() # Final position ptFinal = [[-0.9, 0, 0.45]] oriFinal = [0.01, 0.01, 0.01] diam_goal = [0.05] ur5_robot.add_sphere(ptFinal, diam_goal, ColorRGBA(0.0, 1.0, 0.0, 0.8)) # CPA Parameters err = diam_goal[0] / 4 # Max error allowed max_iter = 2500 # Max iterations zeta = [0.5 for i in range(7)] # Attractive force gain of each obstacle eta = [0.00006 for i in range(6)] # Repulsive gain of each obstacle rho_0 = [i / 2 for i in diam_obs] # Influence distance of each obstacle dist_att = 0.05 # Influence distance in workspace dist_att_config = 0.15 # Influence distance in configuration space alfa = 0.5 # Learning rate of positioning alfa_rot = 0.4 # Learning rate of orientation CP_ur5_rep = 0.15 # Repulsive fields on UR5 # Joint positions initialization if arg.CSV: ur5_joint_positions_vec = ur5_robot.joint_states.position # Get current orientation and position of tool0 link q = quaternion_from_matrix(ur5_robot.joint_states.position) oriAtual = q[1], q[2], q[3], q[0] ptAtual = get_ur5_position(ur5_robot.ur5_param, ur5_robot.joint_states.position, "tool0") hz = get_param("rate", 60) r = rospy.Rate(hz) dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal[0])) n = 0 # Choose to display the path ur5_robot.pose.header.frame_id = "path" raw_input("' =========== Aperte enter para iniciar o algoritmo dos CPAs") while dist_EOF_to_Goal > err and not rospy.is_shutdown() and n < max_iter: # Get UR5 Jacobian of each link Jacobian = get_geometric_jacobian(ur5_robot.ur5_param, ur5_robot.joint_states.position) # Get position and distance from each link to each obstacle CP_pos, CP_dist = ur5_robot.get_repulsive_cp( obs_pos, ur5_robot.joint_states.position, CP_ur5_rep) # Get attractive linear and angular forces and repulsive forces joint_att_force_p, joint_att_force_w, joint_rep_force = CPA.get_joint_forces( ptAtual, ptFinal, oriAtual, oriFinal, dist_EOF_to_Goal, Jacobian, ur5_robot.joint_states.position, ur5_robot.ur5_param, zeta, eta, rho_0, dist_att, dist_att_config, CP_dist, CP_pos, obs_pos, arg.APF, CP_ur5_rep) # Joint angles UPDATE - Attractive force ur5_robot.joint_states.position = ur5_robot.joint_states.position + \ alfa * joint_att_force_p[0] if arg.OC_Off: ur5_robot.joint_states.position = ur5_robot.joint_states.position + \ alfa_rot * joint_att_force_w[0] # Joint angles UPDATE - Repulsive force list = np.transpose(joint_rep_force[0]).tolist() for j in range(6): for i in range(6): ur5_robot.joint_states.position[i] = ur5_robot.joint_states.position[i] + \ alfa * list[j][i] # Get current orientation of tool0 link q = quaternion_from_matrix(ur5_robot.joint_states.position) oriAtual = q[1], q[2], q[3], q[0] # Get current position of tool0 link ptAtual = get_ur5_position(ur5_robot.ur5_param, ur5_robot.joint_states.position, "tool0") # Angle offset between tool0 and base_link (base?) oriAtual += quaternion_from_euler(1.5707, 1.5707, 0) # Get distance from EOF to goal dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) # If true, publish topics to transform into csv later on if arg.CSV: # ur5_joint_positions_vec.append(np.concatenate(([n+1], ur5_robot.joint_states.position), 0)) ur5_joint_positions_vec = np.vstack( (ur5_joint_positions_vec, ur5_robot.joint_states.position)) # If true, publish topics to publish_trajectory.py in order to see the path in RVIZ if arg.plot: print("aqui") if n % 100 == 0: # ur5_robot.visualize_path_planned(ptAtual) ur5_robot.pose.pose.position.x = ptAtual[0] ur5_robot.pose.pose.position.y = ptAtual[1] ur5_robot.pose.pose.position.z = ptAtual[2] ur5_robot.pose_publisher.publish(ur5_robot.pose) way_points.append(ur5_robot.joint_states.position) try: r.sleep() except rospy.exceptions.ROSTimeMovedBackwardsException: pass n += 1 print("Iterations: " + str(n)) print("Distance to goal: " + str(dist_EOF_to_Goal)) if arg.CSV: with open( '/home/caio/3_Projetos_UR5/Project_IFAC_2019/src/custom_codes/csv_files/Joint_states.csv', mode='w') as employee_file: employee_writer = csv.writer(employee_file, delimiter=' ', quotechar='"', quoting=csv.QUOTE_MINIMAL) employee_writer.writerow([ 'Index', 'Joint1', 'Joint2', 'Joint3', 'Joint4', 'Joint5', 'Joint6' ]) pos = ur5_joint_positions_vec n = 0 for position in pos: employee_writer.writerow([ n, position[0], position[1], position[2], position[3], position[4], position[5] ]) n += 1 if arg.plot: # choose the trajectory to display in RVIZ ur5_robot.pose.header.frame_id = "trajectory" ur5_robot.pose_publisher.publish(ur5_robot.pose) raw_input("' =========== Press enter to send the trajectory to Gazebo \n") ur5_robot.move(way_points, "gazebo") raw_input( "' =========== Aperte enter para posicionar o UR5 na posicao inicial\n" ) ur5_robot.move(([0, -1.5707, 0, -1.5707, 1.5707, 0], ), "gazebo") if arg.plot: # Stop plotting trajectory ur5_robot.pose.header.frame_id = "end" ur5_robot.pose_publisher.publish(ur5_robot.pose) if args.realUR5: raw_input( "' =========== Aperte enter para posicionar o UR5 real na posicao UP\n" ) ur5_robot.move(([0, -1.5707, 0, -1.5707, 0, 0], ), "real") raw_input( "' =========== Aperte enter para enviar a trajetoria para o UR5 !!!REAL!!! \n" ) ur5_robot.move(way_points, "real")
def main(): ur5_robot = MoveGroupPythonIntefaceTutorial() way_points = [] # UR5 Initial position raw_input("' =========== Aperte enter para posicionar o UR5 \n") # Posicao configurada no fake_controller_joint_states ur5_robot.joint_states.position = [0, -1.5707, 0, -1.5707, 1.5707, 0] way_points.append(ur5_robot.joint_states.position) ur5_robot.move(way_points, "gazebo") raw_input("' =========== Aperte enter para carregar os param. dos CPAs \n") # Obstacle positions oc = [-0.9, 0, 0.375] # Obstacle reference point - 3D printer d1 = -0.080 s = 1 obs_pos = [ oc, np.add(oc, [s * 0.14, 0.0925, 0.255 + d1]), np.add(oc, [s * 0.14, 0.185, 0.255 + d1]), np.add(oc, [s * 0.14, 0, 0.255 + d1]), np.add(oc, [s * 0.14, -0.0925, 0.255 + d1]), np.add(oc, [s * 0.14, -0.185, 0.255 + d1]), np.add(oc, [s * 0.14, -0.185, 0.16 + d1]), np.add(oc, [s * 0.14, 0.185, 0.16 + d1]), np.add(oc, [s * 0.14, 0.0925, 0.05 + d1]), np.add(oc, [s * 0.14, 0.185, 0.05 + d1]), np.add(oc, [s * 0.14, 0, 0.05 + d1]), np.add(oc, [s * 0.14, -0.0925, 0.05 + d1]), np.add(oc, [s * 0.14, -0.185, 0.05 + d1]) ] diam_obs = [0.18] * len(obs_pos) # Main obstacle repulsive field diam_obs[0] = 0.3 ur5_robot.add_sphere(obs_pos, diam_obs, ColorRGBA(1.0, 0.0, 0.0, 0.5)) # Final position ptFinal = [[-0.9, 0, 0.45]] oriFinal = [0.01, 0.01, 0.01] diam_goal = [0.05] ur5_robot.add_sphere(ptFinal, diam_goal, ColorRGBA(0.0, 1.0, 0.0, 0.8)) # CPA Parameters err = diam_goal[0] / 4 # Max error allowed max_iter = 2500 # Max iterations zeta = [0.5 for i in range(7)] # Attractive force gain of each obstacle eta = [0.00006 for i in range(6)] # Repulsive gain of each obstacle rho_0 = [i / 2 for i in diam_obs] # Influence distance of each obstacle dist_att = 0.05 # Influence distance in workspace dist_att_config = 0.15 # Influence distance in configuration space alfa = 0.5 # Learning rate of positioning alfa_rot = 0.4 # Learning rate of orientation CP_ur5_rep = 0.15 # Repulsive fields on UR5 # Parameters CPAA_state = True # If True, it enables CPAA only on end effector Orientation_state = True # If True, UR5 will keep end effector orientation # Get current orientation and position of tool0 link q = quaternion_from_matrix(ur5_robot.joint_states.position) oriAtual = q[1], q[2], q[3], q[0] ptAtual = get_ur5_position(ur5_robot.ur5_param, ur5_robot.joint_states.position, "tool0") hz = get_param("rate", 60) r = rospy.Rate(hz) dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal[0])) n = 0 raw_input("' =========== Aperte enter para iniciar o algoritmo dos CPAs") while dist_EOF_to_Goal > err and not rospy.is_shutdown() and n < max_iter: # Get UR5 Jacobian of each link Jacobian = get_geometric_jacobian(ur5_robot.ur5_param, ur5_robot.joint_states.position) # Get position and distance from each link to each obstacle CP_pos, CP_dist = ur5_robot.get_repulsive_cp( obs_pos, ur5_robot.joint_states.position, CP_ur5_rep) # Get attractive linear and angular forces and repulsive forces joint_att_force_p, joint_att_force_w, joint_rep_force = CPA.get_joint_forces( ptAtual, ptFinal, oriAtual, oriFinal, dist_EOF_to_Goal, Jacobian, ur5_robot.joint_states.position, ur5_robot.ur5_param, zeta, eta, rho_0, dist_att, dist_att_config, CP_dist, CP_pos, obs_pos, CPAA_state, CP_ur5_rep) # Joint angles UPDATE - Attractive force ur5_robot.joint_states.position = ur5_robot.joint_states.position + \ alfa * joint_att_force_p[0] if Orientation_state: ur5_robot.joint_states.position = ur5_robot.joint_states.position + \ alfa_rot * joint_att_force_w[0] way_points.append(ur5_robot.joint_states.position) # Joint angles UPDATE - Repulsive force list = np.transpose(joint_rep_force[0]).tolist() for j in range(6): for i in range(6): ur5_robot.joint_states.position[i] = ur5_robot.joint_states.position[i] + \ alfa * list[j][i] # Get current orientation of tool0 link q = quaternion_from_matrix(ur5_robot.joint_states.position) oriAtual = q[1], q[2], q[3], q[0] # Get current position of tool0 link ptAtual = get_ur5_position(ur5_robot.ur5_param, ur5_robot.joint_states.position, "tool0") # Angle offset between tool0 and base_link (base?) oriAtual += quaternion_from_euler(1.5707, 1.5707, 0) # Get distance from EOF to goal dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) if n % 10 == 0: ur5_robot.visualize_path_planned(ptAtual) # ur5_robot.add_sphere2(ptAtual, ur5_robot.id2, 0.04, ColorRGBA(0.0, 1.0, 0.0, 0.8)) # plot path as spheres try: r.sleep() except rospy.exceptions.ROSTimeMovedBackwardsException: pass n += 1 print("Iterations: " + str(n)) print("Distance to goal: " + str(dist_EOF_to_Goal)) raw_input("' =========== Press enter to send the trajectory to Gazebo \n") ur5_robot.move(way_points, "gazebo") # UR5 Initial position raw_input( "' =========== Aperte enter para posicionar o UR5 na posicao UP\n") ur5_robot.move(([0, -1.5707, 0, -1.5707, 1.5707, 0], ), "gazebo")
def CPA_vel_control(self): # At the end, the disciplacement will take place as a final orientation Displacement = [0.04, 0.04, 0.04] # CPA Parameters zeta = [0.5 for i in range(7)] # Attractive force gain of the goal dist_att = 0.1 # Influence distance in workspace dist_att_config = 0.2 # Influence distance in configuration space alfa_geral = 1.5 # multiply each alfa (position and rotation) equally alfa = 4 * alfa_geral # Grad step of positioning - Default: 0.5 alfa_rot = 4 * alfa_geral # Grad step of orientation - Default: 0.4 # Return the end effector location relative to the base_link ptAtual, _ = self.tf.lookupTransform("base_link", "grasping_link", rospy.Time()) # Get ptFinal published by ar_marker_0 frame and the orientation from grasping_link to ar_marker_0 ptFinal, oriAtual, oriFinal = self.get_tf_param() print "oriFinal (ar_marker_0 from base_link): ", oriFinal print "oriAtual (grasping_link from ar_marker_0): ", oriAtual, " \n" # Calculate the correction of the orientation relative to the actual orientation R, P, Y = -1 * oriAtual[0], -1 * oriAtual[1], 0.0 corr = [R, P, Y] print "Correction angle (from quat multiply): ", corr # Calculate the distance between end effector and goal dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) # Frequency of the velocity controller pubisher # Max frequency: 125 Hz rate = rospy.Rate(125) raw_input("\n==== Press enter to start APF") while not rospy.is_shutdown(): # Get UR5 Jacobian of each link Jacobian = get_geometric_jacobian(self.ur5_param, self.actual_position) # Get absolute orientation error err_ori = np.sum(oriAtual) # In order to keep orientation constant, we need to correct the orientation # of the end effector in respect to the ar_marker_0 oriAtual = [oriAtual[i] + corr[i] for i in range(len(corr))] print "Angle error (RPY): ", oriAtual # Get attractive linear and angular forces and repulsive forces joint_att_force_p, joint_att_force_w = \ self.get_joint_forces(ptAtual, ptFinal, oriAtual, Displacement, dist_EOF_to_Goal, Jacobian, zeta, dist_att, dist_att_config, err_ori) # Publishes joint valocities related to position only self.joint_vels.data = np.array(alfa * joint_att_force_p[0]) # If orientation control is turned on, sum actual position forces to orientation forces if self.args.OriON: self.joint_vels.data = self.joint_vels.data + \ alfa_rot * joint_att_force_w[0] self.pub_vel.publish(self.joint_vels) # Return the end effector location relative to the base_link ptAtual, _ = self.tf.lookupTransform("base_link", "grasping_link", rospy.Time()) # Check wrist_1_link position just for safety wristPt, _ = self.tf.lookupTransform("base_link", "wrist_1_link", rospy.Time()) # Function to ensure safety. It does not allow End Effector to move below 20 cm above the desk self.safety_stop(ptAtual, wristPt) # Get ptFinal published by ar_marker_0 frame and the orientation from grasping_link to ar_marker_0 # The oriFinal needs to be tracked online because the object will be dynamic ptFinal, oriAtual, oriFinal = self.get_tf_param() print "oriFinal: ", oriFinal print "oriAtual: ", oriAtual, " \n" # Calculate the distance between end effector and goal dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(ptFinal)) rate.sleep()
def CPA_loop(self, args, way_points, R, P, Y, AAPF_COMP=False, ORI_COMP=False): '# add_obstacles(name, height, radius, pose, orientation, r, g, b):' # self.add_obstacles("up", 0.54, 0.09, [-0.76, 0, 0.345], [1.5707, 1.5707, 0], 1, 0, 0) # self.add_obstacles("bottom", 0.54, 0.09, [-0.76, 0, 0.55], [1.5707, 1.5707, 0], 1, 0, 0) # self.add_obstacles("right", 0.35, 0.09, [-0.76, 0.185, 0.455], [0, 0, 0], 1, 0, 0) # self.add_obstacles("left", 0.35, 0.09, [-0.76, -0.185, 0.455], [0, 0, 0], 1, 0, 0) self.add_sphere(self.ptFinal, self.diam_goal, ColorRGBA(0.0, 1.0, 0.0, 1.0)) # apply obstacle colors to moveit self.scene.sendColors() # Final position Displacement = [0.01, 0.01, 0.01] # CPA Parameters err = self.diam_goal[0] / 2 # Max error allowed max_iter = 1500 # Max iterations zeta = [0.5 for i in range(7)] # Attractive force gain of each obstacle rho_0 = [i / 2 for i in self.diam_obs] # Influence distance of each obstacle dist_att = 0.05 # Influence distance in workspace dist_att_config = 0.2 # Influence distance in configuration space alfa = 0.5 # Grad step of positioning - Default: 0.5 alfa_rot = 0.05 # Grad step of orientation - Default: 0.4 CP_ur5_rep = [0.15] * 6 # Repulsive fields on UR5 CP_ur5_rep[-2] = 0.15 if args.AAPF and not args.OriON or (AAPF_COMP and not ORI_COMP): self.eta = [0.0006 for i in range(6)] ptAtual = get_ur5_position(self.ur5_param, self.joint_states.position, "grasping_link") hz = get_param("rate", 120) r = rospy.Rate(hz) dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(self.ptFinal[0])) dist_EOF_to_Goal_vec = dist_EOF_to_Goal n = 0 err_ori = 1 corr = [R, P, Y] # Get current orientation and position of grasping_link ur5_joint_positions_vec = self.joint_states.position joint_attractive_forces = np.zeros(6) joint_rep_forces = np.zeros(6) ori_atual_vec = np.zeros(3) while (dist_EOF_to_Goal > err or abs(err_ori) > 0.02 ) and not rospy.is_shutdown() and n < max_iter: # Get UR5 Jacobian of each link Jacobian = get_geometric_jacobian(self.ur5_param, self.joint_states.position) # Get position and distance from each link to each obstacle CP_pos, CP_dist = self.get_repulsive_cp(self.obs_pos, self.joint_states.position, CP_ur5_rep) oriAtual = euler_from_matrix(self.matrix_from_joint_angles()) oriAtual = [oriAtual[i] + corr[i] for i in range(len(corr))] # Get attractive linear and angular forces and repulsive forces joint_att_force_p, joint_att_force_w, joint_rep_force = \ CPA.get_joint_forces(args, ptAtual, self.ptFinal, oriAtual, Displacement, dist_EOF_to_Goal, Jacobian, self.joint_states.position, self.ur5_param, zeta, self.eta, rho_0, dist_att, dist_att_config, CP_dist, CP_pos, self.obs_pos, CP_ur5_rep, AAPF_COMP) joint_rep_force = np.clip(joint_rep_force, -0.1, 0.1) # Joint angles UPDATE - Attractive force self.joint_states.position = self.joint_states.position + \ alfa * joint_att_force_p[0] if args.OriON or ORI_COMP: self.joint_states.position = self.joint_states.position + \ alfa_rot * joint_att_force_w[0] # Joint angles UPDATE - Repulsive force list = np.transpose(joint_rep_force[0]).tolist() for j in range(6): for i in range(6): self.joint_states.position[i] = self.joint_states.position[i] + \ alfa * list[j][i] matrix = self.matrix_from_joint_angles() # Get current position of grasping_link ptAtual = get_ur5_position(self.ur5_param, self.joint_states.position, "grasping_link") if args.plotPath: # Visualize path planned in RVIZ self.visualize_path_planned(ptAtual) # Get absolute orientation error err_ori = np.sum(oriAtual) # Get distance from EOF to goal dist_EOF_to_Goal = np.linalg.norm(ptAtual - np.asarray(self.ptFinal)) # If true, publish topics to transform into csv later on if args.CSV: ur5_joint_positions_vec = np.vstack( (ur5_joint_positions_vec, self.joint_states.position)) dist_EOF_to_Goal_vec = np.vstack( (dist_EOF_to_Goal_vec, dist_EOF_to_Goal)) joint_attractive_forces = np.vstack( (joint_attractive_forces, joint_att_force_p)) joint_rep_forces = np.vstack( (joint_rep_forces, list[-1] )) # list[-1] corresponds to rep forces on the last joint ori_atual_vec = np.vstack((ori_atual_vec, oriAtual)) # If true, publish topics to publish_trajectory.py in order to see the path in RVIZ # if n % 10 is used to reduced the total number of waypoints generated by APF or AAPF if args.plot: if n % 1 == 0: self.pose.pose.position.x = ptAtual[0] self.pose.pose.position.y = ptAtual[1] self.pose.pose.position.z = ptAtual[2] self.pose_publisher.publish(self.pose) # Save way points in order to send it to gazebo way_points.append(self.joint_states.position) try: r.sleep() except rospy.exceptions.ROSTimeMovedBackwardsException: pass n += 1 return way_points, n, dist_EOF_to_Goal, ur5_joint_positions_vec, dist_EOF_to_Goal_vec, joint_attractive_forces, joint_rep_forces, ori_atual_vec