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