def RRT(target_x, config0, NIter=10000):
nodes = [Node(config0)]
start_x = forward_kinematics(config0)
for i in xrange(NIter):
config_random = generate_random_config()
nearest_node_index = find_nearest_node(nodes, config_random)
config_new = step_from_toward(nodes[nearest_node_index].config,
config_random)
# Use for test here
'''
print "New Configuration: ", config_new
print "in_workspace(config_new) ", in_workspace(config_new)
print "in_collision(config_new) ", in_collision(config_new,obs)
print "path_in_collision(config_new) ", path_in_collision(config_new, nodes[nearest_node_index].config, obs)
'''
if in_workspace(config_new) and not in_collision(
config_new, obs) and not path_in_collision(
config_new, nodes[nearest_node_index].config, obs):
new_node = Node(config_new, nearest_node_index)
nodes.append(new_node)
print "Add new node, list length: ", len(nodes)
# print "Distance: ", S_dist(forward_kinematics(config_new),target_x)
if S_dist(forward_kinematics(config_new),
target_x) < TARGET_RADIUS:
plan = backtrace(nodes, new_node)
return plan
# for loop end
return None # no plan found
def forward_kinematics(self, configuration):
"""Forward kinematics function.
Args:
configuration (Configuration): the 6 joint angles in radians
Returns:
(frame): The tool0 frame in robot coordinate system (RCS).
"""
return forward_kinematics(configuration, self.params)
def update(self, left_pos, right_pos):
delta_left_pos = left_pos - self.prev_left_pos
delta_right_pos = right_pos - self.prev_right_pos
#print(str(delta_left_pos) + "\t" + str(delta_right_pos))
velocity = kinematics.forward_kinematics(delta_left_pos,
delta_right_pos)
#velocity.print()
self.pose = kinematics.integrate_forward_kinematics(
self.previous_pose, velocity)
self.prev_left_pos = left_pos
self.prev_right_pos = right_pos
self.previous_pose = self.pose
def publish_data(self, time_obj):
if len(self.left_positions) <= 10 or len(self.right_positions) <= 10:
return
while self.left_positions[-1][1].to_sec() - self.left_positions[0][1].to_sec() > 1:
self.left_positions.pop(0)
while self.right_positions[-1][1].to_sec() - self.right_positions[0][1].to_sec() > 1:
self.right_positions.pop(0)
if len(self.left_positions) <= 10 or len(self.right_positions) <= 10:
return
out_msg = Odometry()
out_msg.header.stamp = rospy.Time.now()
out_msg.child_frame_id = "base_link"
try:
if len(self.left_positions) > 2 and len(self.right_positions) > 2:
left_vel = self.linear_reg_slope(self.left_positions)
right_vel = self.linear_reg_slope(self.right_positions)
v, omega = kinematics.forward_kinematics(left_vel, right_vel, track=self.track, diameter=self.wheel_diameter)
out_msg.twist.twist.linear.x = v
out_msg.twist.twist.angular.z = omega
# don't use old data
self.left_positions.pop(0)
self.right_positions.pop(0)
# rospy.logerr("motor_to_twist setting twists: " + str(v) + " " + str(omega))
else:
# if no data is being recieved then the motor control node is probably not running. Publish zero velocity.
out_msg.twist.twist.linear.x = 0
out_msg.twist.twist.angular.z = 0
self.set_covariance(out_msg)
self.pub.publish(out_msg)
# rospy.logerr("motor_to_twist published")
except:
pass
for s,d in zip(pw,destination):
# rospy.loginfo(make_trajectory(s,d))
pos_list.extend(make_trajectory(s,d))
rospy.loginfo(pos_list)
else:
pos_list = []
# rospy.loginfo(estimator.position_camera)
# pos_list = [point1[:3]+np.array([0,0,0.05]),point1[:3],point1[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.02]),point2[:3]+np.array([0,0,0.05])]
grip = [False,True,True,True,False,False] * len(pw)
yaw = [0]*12 + [0]*3 + [np.pi/2]*3 + [0]*3 + [np.pi/2]*3
rospy.loginfo(pos_list)
for pos,y in zip(pos_list,yaw):
q = kin.inverse_kinematics(pos,y)
print(kin.forward_kinematics(q)[0]["joint_4"])
if q!=None:
target_joints.append(q)
else:
print("No solution")
exit()
arm_controller.home_arm()
arm_controller.open()
for joint,g in zip(target_joints,grip):
arm_controller.set_joint_state(joint)
while(not arm_controller.has_converged()):
pass
if g == True:
arm_controller.close()
else:
joints = [
joint(0, np.array([0, 0, 1]), [-180, 180], [0, 1]),
joint(0, np.array([0, 1, 0]), [-138, 64], [1, 2]),
joint(0, np.array([0, 1, 0]), [-124, 124], [2, 3]),
joint(0, np.array([0, 0, 1]), [-180, 180], [3, 4]),
joint(0, np.array([0, 1, 0]), [-140, 141], [4, 5]),
joint(0, np.array([0, 0, 1]), [-math.inf, math.inf], [5, -1])
]
r6Robot = robot(links, joints)
#forward forward_kinematics
T = forward_kinematics(
np.array([
deg_to_rad(45),
deg_to_rad(-45),
deg_to_rad(-45),
deg_to_rad(45),
deg_to_rad(0),
deg_to_rad(-45)
]), r6Robot)
print("T = ")
print(T)
print()
#inverse_kinematics
q = inverse_kinematics(T)
print("joints")
print([i / math.pi * 180 for i in q])
print()
#check
from kinematics import forward_kinematics, jacobian
import numpy as np
import numpy.linalg as la
import math as m
import matplotlib.pyplot as plt
if __name__ == '__main__':
q1_vec = np.linspace(0, m.pi / 2, 201)
q2_vec = np.linspace(-m.pi, m.pi, 201)
q3_vec = np.linspace(-m.pi, m.pi, 201)
singular_configurations = [
(q1, q2, q3) for q1 in q1_vec for q2 in q2_vec for q3 in q3_vec
if any(la.svd(jacobian((q1, q2, q3)))[1] < 1e-6)
]
np.savetxt('singular_configurations.csv',
singular_configurations,
delimiter=',')
singular_points = np.array(
[forward_kinematics(q)[0] for q in singular_configurations])
fig, ax = plt.subplots()
ax.scatter(singular_points[:, 0], singular_points[:, 1], s=1)
ax.set_xlabel('x-axis')
ax.set_ylabel('y-axis')
ax.grid()
fig.savefig('singularities.png')
def path_in_collision(q1, q2, obstacles):
endpoint1 = forward_kinematics(q1)
endpoint2 = forward_kinematics(q2)
seg = Segment(endpoint1,endpoint2)
return isIntersect(seg,obstacles)
while (not arm_controller.has_converged()):
pass
rospy.sleep(0.5)
#get position and orientation from AR tag in camera frame
cam_pos = cam.ar_position
cam_orient = cam.ar_orient
#create Homogenious matrix of AR tag in camera frame and append in list
cam_orient_rot = eulerAnglesToRotationMatrix(
tf.transformations.euler_from_quaternion(cam_orient))
cam_H = np.eye(4)
cam_H[:3, :3] = cam_orient_rot
cam_H[:3, 3] = cam_pos
cam_H_list.append(cam_H)
#compute Homogenious matrix of ar tag in world frame and append in list
arm_H = np.dot(
kin.forward_kinematics(joint)[1][-1],
translation_matrix((0.029, -0.001, 0.043)))
arm_H_list.append(arm_H)
rospy.sleep(0.5)
#save homogenious matrices in pickle file
pickle.dump(arm_H_list, open("arm_H_pos.p", "wb"))
pickle.dump(cam_H_list, open("cam_H_pos.p", "wb"))
#load pickle files and compute transform
arm_H_list = pickle.load(open("arm_H_pos.p", "rb"))
cam_H_list = pickle.load(open("cam_H_pos.p", "rb"))
H = compute_transformation(arm_H_list, cam_H_list)
def in_workspace(config):
x = forward_kinematics(config)
return abs(x[0]) <= XDIM and abs(x[1]) <= YDIM and abs(x[2]) <= ZDIM
def test_Initial():
print " ########## Initial State ########## "
print " forward_kinematics(config0): "
print " ", forward_kinematics(config0)
print " in_collision(config0,obs): ", in_collision(config0, obs)
