def move(client, JOINT_NAMES, index, tags2base, speed=8):
"""
Figures out where to move. Split it into an XY move and a Z move. Repeat XY move until correct
Args:
client -
JOINT_NAMES -
index -
tags2base -
speed -
"""
current_state = np.array(
rospy.wait_for_message("joint_states", JointState).position)
q_des = tags2base[index, :]
q_current = KIN.fk(current_state)
# We only care about XY motion
q_des = np.array([q_des[0], q_des[1], q_current[2]])
q_des[0] = q_des[0] + 0.03 - 0.004 * int(index / 5)
# Define a new one that we can change
move_sub(client, current_state, JOINT_NAMES, q_des, speed)
def set_initial_pose(self, cur_pos):
if (cur_pos == None):
return
self.times[0] = 0
self.elbow_up[0] = kin.get_elbow(cur_pos)
cur_pos_workspace = kin.fk(cur_pos)
print cur_pos_workspace
for i in range(0, 5):
self.waypoints[i][0] = cur_pos_workspace[0]
self.initialPoseSet = True
def __init__(self):
global NODE_NAME, NaN, PI
global GROUP_NAME, GROUP_SIZE, FAMILY_NAME, NAME_1, NAME_2, NAME_3, NAME_4
global ACTION_SERVER_NAME
# initialize node
rospy.init_node(NODE_NAME, anonymous=True)
# Feedback from hebiros
self.hebi_fb = None
# initialize services
# Service to set command lifetime
self.set_command_lifetime_client = rospy.ServiceProxy('/hebiros/' + \
GROUP_NAME + '/set_command_lifetime', SetCommandLifetimeSrv)
# Service to get entry list
self.entry_list_client = rospy.ServiceProxy('/hebiros/entry_list',
EntryListSrv)
# Add grout from names
self.add_group_client = rospy.ServiceProxy(\
'/hebiros/add_group_from_names', AddGroupFromNamesSrv)
# Get size of group
self.size_client = rospy.ServiceProxy(
'/hebiros/' + GROUP_NAME + '/size', SizeSrv)
# Initialize subscribers
# Subscribe to hebi fb joint state
rospy.Subscriber('/hebiros/' + GROUP_NAME + \
'/feedback/joint_state', JointState, self.hebi_fb_cb)
# Initialize hebi group
self.hebi_lookup()
# Initialize h
while (not rospy.is_shutdown()):
rospy.loginfo(kin.fk(self.hebi_fb.position))
rospy.sleep(0.5)
def move_sub(client, joint_states, JOINT_NAMES, q_des, speed):
"""
Actually moves the arm
"""
q_current = KIN.fk(joint_states)
q_ik = q_des
# print "Goal: ", q_des
while True:
final_states = KIN.ik(q_ik)
## Hacky fixes
# Fix theta4
final_states[3] = -final_states[2] - final_states[1] - math.pi / 2
# Stop theta1 from going all the way around
if final_states[0] > 0:
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] - math.pi * 2)):
final_states[0] = final_states[0] - 2 * math.pi
elif final_states[0] < 0:
# print "yo"
# print abs(joint_states[0] - final_states[0])
# print abs(joint_states[0] - (final_states[0] + math.pi*2))
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] + math.pi * 2)):
final_states[0] = final_states[0] + 2 * math.pi
q_current = KIN.fk(final_states)
# print "Current p:", q_current, "Distance: ", np.linalg.norm(q_current - q_des)
if np.linalg.norm(q_current - q_des) <= 0.001:
break
else:
q_ik = q_ik + (q_des - q_current) / 2
# raw_input("Check before moving")
# print final_states
# raw_input()
# Actually move
move_speed = speed
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
g.trajectory.points = [
JointTrajectoryPoint(positions=joint_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=final_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(move_speed))
]
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def test_Z(client, JOINT_NAMES, zTest, tfListener):
"""
Reads the force vs. time values from the sensor and sets the tested block to wood (1) or foam (0)
"""
global forceReading
#Create instance of force sensor subscriber
force_sensor_sub = rospy.Subscriber("ati_ft_data", Wrench,
force_sensor_callback)
#Get current robot joint angles
joint_states = np.array(
rospy.wait_for_message("joint_states", JointState).position)
# print "Initial position", KIN.fk(joint_states)
#Solve XY IK solution
# (trans, rot) = tfListener.lookupTransform('base','ee_link', rospy.Time(0))
z_des = zTest
z_ik = z_des
while True:
final_states = KIN.move_z(z_ik, np.array(joint_states))
z_current = KIN.fk(final_states)[2]
if abs(z_current - z_des) <= 0.001:
break
else:
z_ik = z_ik + (z_des - z_current) / 2
# print z_current
# print "Predicted position", KIN.fk(final_states)
# print final_states
if final_states[0] > 0:
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] - math.pi * 2)):
final_states[0] = final_states[0] - 2 * math.pi
elif final_states[0] < 0:
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] + math.pi * 2)):
final_states[0] = final_states[0] + 2 * math.pi
time.sleep(
.2) #This allows the subscriber to obtain the first force reading
#Set movement speed
#This will be MUCH slower so we will set a velocity variable here to calculate time
#z_dist = 2 in travel
move_time_slow = 8.0
move_time = 0.5
##SENSOR sample time == approx. 0.008 seconds
#Get force value from sensor
vector_force = forceReading.force
value_force_calib = np.array(
[vector_force.x, vector_force.y, vector_force.z])
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
g.trajectory.points = [
JointTrajectoryPoint(positions=joint_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(
positions=final_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(move_time_slow))
]
#Do not insert a wait_for_result in order to allow for interrupts
client.send_goal(g)
# client.wait_for_result()
start_time = time.time()
value_force = 0
maxForce = 0
force_threshold = 10000
force_min_thresh = 10
wood_force_thresh = 250
detectedWood = 0
forcePts = 20
forceArray = np.zeros(forcePts)
i = 0
# continuously read the force values in a loop
while True:
init_force = np.array([
forceReading.force.x, forceReading.force.y,
forceReading.force.z
])
value_force = np.linalg.norm(init_force - value_force_calib)
# Check if over start threshold and haven't already collected enough points
if value_force > force_min_thresh and i < forcePts:
# print "Z force", value_force
# print "Total force", total_force
forceArray[i] = value_force
i = i + 1
time.sleep(.005)
# If we've collected enough points check the sloope
elif i >= forcePts:
# print value_force
# print forceArray
avg_force = np.mean(forceArray)
print "Average Force", avg_force
if avg_force > wood_force_thresh:
detectedWood = 1
# print 'This is wood'
else:
detectedWood = 0
# print "This is foam"
break
# After testing stop the robot
client.cancel_goal()
# Raise the end effector back up
curr_state = rospy.wait_for_message("joint_states",
JointState).position
g.trajectory.points = [
JointTrajectoryPoint(positions=curr_state,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=joint_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(move_time))
]
client.send_goal(g)
client.wait_for_result()
# Return whether or not we detected wood.
return detectedWood
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def move_Z(client, JOINT_NAMES, zTest):
#Get current robot joint angles
joint_states = np.array(
rospy.wait_for_message("joint_states", JointState).position)
#Solve XY IK solution
z_des = zTest
z_current = KIN.fk(joint_states)[2]
z_ik = z_des - z_current
# print "z_ik", z_ik
while True:
final_states = KIN.move_z(z_ik, np.array(joint_states))
z_current = KIN.fk(final_states)[2]
# print "z_current", z_current
# raw_input()
if abs(z_current - z_des) <= 0.001:
break
else:
# print "z_des"
z_ik = (z_des - z_current) / 2
# print "z_ik", z_ik
# raw_input("It's gunna move now")
# print final_states
if final_states[0] > 0:
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] - math.pi * 2)):
final_states[0] = final_states[0] - 2 * math.pi
elif final_states[0] < 0:
# print "yo"
# print abs(joint_states[0] - final_states[0])
# print abs(joint_states[0] - (final_states[0] + math.pi*2))
if abs(joint_states[0] -
final_states[0]) > abs(joint_states[0] -
(final_states[0] + math.pi * 2)):
final_states[0] = final_states[0] + 2 * math.pi
# print final_states
# raw_input()
#Set movement speed
#This will be MUCH slower so we will set a velocity variable here to calculate time
#z_dist = 2 in travel
# move_time_slow = 5.0
move_time = 2.0
g = FollowJointTrajectoryGoal()
g.trajectory = JointTrajectory()
g.trajectory.joint_names = JOINT_NAMES
try:
g.trajectory.points = [
JointTrajectoryPoint(positions=joint_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(0.0)),
JointTrajectoryPoint(positions=final_states,
velocities=[0] * 6,
time_from_start=rospy.Duration(move_time))
]
#Do not insert a wait_for_result in order to allow for interrupts
client.send_goal(g)
client.wait_for_result()
except KeyboardInterrupt:
client.cancel_goal()
raise
except:
raise
def action_server_cb(self, goal):
rospy.loginfo(IDSTR + "Action server cb called")
rospy.loginfo(goal)
names = [FAMILY_NAME+"/"+NAME_1,FAMILY_NAME+"/"+NAME_2,
FAMILY_NAME+"/"+NAME_3,FAMILY_NAME+"/"+NAME_4]
# get current state
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
# Determine requested elbow position
req_elbow = cur_elbow
if (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA):
req_elbow = goal.waypoint_1[0] >= 0
# Create trajectory generaor object
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
# Change elbow position if necessary
# Will end on rest position of opposite elbow
if( False and (goal.type == TYPE_TARGET or goal.type == TYPE_CAMERA)
and not cur_elbow == req_elbow):
rospy.loginfo(IDSTR + "Switching elbow config")
# get rest pos after transition (ie opposite of current elbow)
rest_pos = []
if (cur_elbow):
rest_pos = REST_POS_ELBOW_DOWN
else:
rest_pos = REST_POS_ELBOW_UP
# time to travel from cur pose to VERT_POS
travel_time_1 = dist(kin.fk(cur_pose), VERT_POS) / SPEED_TRAVEL
# time to travel from vert_pos to rest pos
travel_time_2 = dist(VERT_POS, rest_pos) / SPEED_TRAVEL
t.addWaypoint(VERT_POS, 10, cur_elbow)
# t.addWaypoint(VERT_POS, 10, not cur_elbow)
# t.addWaypoint(VERT_POS, travel_time_1, cur_elbow)
# t.addWaypoint(VERT_POS, VERT_ELBOW_TRANSITION_TIME, not cur_elbow)
# send trajectory to switch in vert pose
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 1")
# send trajectory to go to rest pose
cur_pose = self.hebi_fb.position
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
t.addWaypoint(VERT_POS, 10, not cur_elbow)
t.addWaypoint(rest_pos, 10, not cur_elbow)
hebi_goal = t.createTrajectory()
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
while (not self.hebi_is_done):
pass
rospy.loginfo(IDSTR + "done switching elbow config part 2")
# redo intial setup
cur_pose = self.hebi_fb.position
cur_elbow = kin.get_elbow(cur_pose)
t = TrajectoryGenerator(names)
t.set_initial_pose(cur_pose)
if(goal.type == TYPE_TARGET):
# create waypoint to go to before goal.waypoint_1
approach_waypoint = list(goal.waypoint_1);
rough_approach_time = 0
if(goal.approach_from_above):
approach_waypoint[1] += VERT_APPROACH_DIST
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# pre_approach_waypoint = approach_waypoint + \
# (VERT_HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
# pre_approach_time = dist(kin.fk(cur_pose), pre_approach_waypoint) / SPEED_TRAVEL
# t.addWaypoint(pre_approach_waypoint, pre_approach_time, req_elbow)
# rough_approach_time = dist(pre_approach_waypoint, approach_waypoint) / SPEED_APPROACH
else:
approach_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_1[0]))
rough_approach_time = dist(kin.fk(cur_pose), approach_waypoint) / SPEED_TRAVEL
# time to get from cur_pose to approach_waypoint
# time to get from approach_waypoint to goal.waypoint_1
fine_approach_time = dist(approach_waypoint, goal.waypoint_1) / SPEED_APPROACH
# create waypoint to go to after goal.waypoint_2
depart_waypoint = list(goal.waypoint_2);
if(goal.approach_from_above):
depart_waypoint[1] += VERT_APPROACH_DIST
else:
depart_waypoint[0] += (HORZ_APPROACH_DIST * np.sign(-goal.waypoint_2[0]))
# time to get from goal.waypoint_2 to depart_waypoint
fine_depart_time = dist(depart_waypoint, goal.waypoint_2) / SPEED_DEPART
# Add waypoints
t.addWaypoint(approach_waypoint, rough_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_1,fine_approach_time, req_elbow)
t.addWaypoint(goal.waypoint_2,goal.duration, req_elbow)
t.addWaypoint(depart_waypoint, fine_depart_time, req_elbow)
if(goal.type == TYPE_REST):
if(cur_elbow):
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_UP) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_UP, travel_time, True)
else:
travel_time = dist(kin.fk(cur_pose),REST_POS_ELBOW_DOWN) / SPEED_TRAVEL
t.addWaypoint(REST_POS_ELBOW_DOWN, travel_time, True)
hebi_goal = None
try:
hebi_goal = t.createTrajectory()
except:
rospy.logwarn("Arm Planner Node FAILED to create trajectory")
self.action_server.setAborted()
return
# rospy.loginfo(goal)
# Send goal to action server
self.hebi_is_done = False
self.trajectory_client.send_goal(hebi_goal,\
self.trajectory_done_cb,\
self.trajectory_active_cb,
self.trajectory_feedback_cb)
# self.as_feedback.percent_complete = 100;
# self.action_server.publish_feedback(self.as_feedback)
#
while (not self.hebi_is_done):
pass
self.action_server.set_succeeded(self.as_result)
cmd.position = kin.ik(wayp, self.elbow_up[index])
position_epsilon = kin.ik(wayp_epsilon, self.elbow_up[index])
for joint in range(0, 4):
cmd.velocity[joint] = (cmd.position[joint] -
position_epsilon[joint]) / epsilon
# Calculate current joint velocity)
# Calculate workspace waypoint a small time in the future
wayp_epsilon = list([0, 0, 0, 0, 0])
for dof in range(0, 5):
difference = next_wayp[dof] - prev_wayp[dof]
wayp[dof] = prev_wayp[dof] + (leg_percentage * leg_duration)
return cmd
if __name__ == '__main__':
t = TrajectoryGenerator(["a", "b", "c", "d"])
t.set_initial_pose(kin.ik([0.2, 0, 0, 0, 0], True))
t.addWaypoint([0.6, 0, 0, 0, 0], 1, True)
time = 0
while (time <= t.getDuration()):
cmd = t.getJointStateCommand(time)
print kin.fk(cmd.position)
print cmd.velocity
print "\n"
time = time + 0.1