def genWaypoints(g_path_rev, w_map):
g_pts = []
w_ori = []
g_path = list(reversed(g_path_rev))
# Waypoints based on change in direction
'''
prev_pt = g_path[0]
i_dx = g_path[1].x - g_path[0].x
i_dy = g_path[1].y - g_path[0].y
heading = (i_dx, i_dy)
g_path.pop(0)
for i, point in enumerate(g_path):
dx = point.x - prev_pt.x
dy = point.y - prev_pt.y
cur_heading = (dx, dy)
if cur_heading != heading and i != len(g_path) - 1:
g_pts.append(prev_pt)
th_heading = math.atan2(heading[1], heading[0])
q_t = quaternion_from_euler(0, 0, th_heading)
quat_heading = Quaternion(*q_t)
w_ori.append(quat_heading)
heading = cur_heading
prev_pt = point
g_pts.append(g_path[-1])
q_t = quaternion_from_euler(0, 0, 0)
quat_heading = Quaternion(*q_t)
w_ori.append(quat_heading)
'''
# Waypoints every 4 grids
prev_pt = g_path[0]
tmp_ctr = 0
for i, point in enumerate(g_path):
if i == len(g_path) - 1:
g_pts.append(g_path[-1])
q_t = quaternion_from_euler(0, 0, 0)
quat_heading = Quaternion(*q_t)
w_ori.append(quat_heading)
elif tmp_ctr == 3:
tmp_ctr = 0
g_pts.append(point)
dx = point.x - prev_pt.x
dy = point.y - prev_pt.y
heading = (dx, dy)
th_heading = math.atan2(heading[1], heading[0])
q_t = quaternion_from_euler(0, 0, th_heading)
quat_heading = Quaternion(*q_t)
w_ori.append(quat_heading)
prev_pt = point
else:
tmp_ctr += 1
# Convert grid points to world coordinates
w_pts = []
for point in g_pts:
w_pts.append(gridToWorld(point, w_map))
# Create actual Path()
path = Path()
for i in range(len(w_pts)):
path.poses.append(PoseStamped(Header(), Pose(w_pts[i], w_ori[i])))
return path
Y = mt.degrees(mt.asin(t2)) t3 = +2.0 * (w * z + x * y) t4 = +1.0 - 2.0 * (ysqr + z * z) Z = mt.degrees(mt.atan2(t3, t4)) return X, Y, Z local_velocity = TwistStamped() def lv_cb(data): global local_velocity local_velocity = data local_position = PoseStamped() def lp_cb(data): global local_position local_position = data def state_cb(data): global current_state current_state = data # flag = data.armed # print(flag) # rospy.loginfo(current_state.connected) def imu_cb(data): global imu_data
def update_wc(self,msg):
v = self.robot.GetActiveDOFValues()
for name in self.joint_names:
v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)]
self.robot.SetActiveDOFValues(v)
rospy.loginfo("I have got a wc location!")
pos_temp = [msg.pose.position.x,
msg.pose.position.y,
msg.pose.position.z]
ori_temp = [msg.pose.orientation.x,
msg.pose.orientation.y,
msg.pose.orientation.z,
msg.pose.orientation.w]
self.pr2_B_wc = createBMatrix(pos_temp, ori_temp)
psm = PoseStamped()
psm.header.stamp = rospy.Time.now()
#self.goal_pose_pub.publish(psm)
self.pub_feedback("Reaching toward goal location")
wheelchair_location = self.pr2_B_wc * self.corner_B_head.I
self.wheelchair.SetTransform(np.array(wheelchair_location))
pos_goal = wheelchair_location[:3,3]
ori_goal = tr.matrix_to_quaternion(wheelchair_location[0:3,0:3])
marker = Marker()
marker.header.frame_id = "base_link"
marker.header.stamp = rospy.Time()
marker.ns = "arm_reacher_wc_model"
marker.id = 0
marker.type = Marker.MESH_RESOURCE;
marker.action = Marker.ADD
marker.pose.position.x = pos_goal[0]
marker.pose.position.y = pos_goal[1]
marker.pose.position.z = pos_goal[2]
marker.pose.orientation.x = ori_goal[0]
marker.pose.orientation.y = ori_goal[1]
marker.pose.orientation.z = ori_goal[2]
marker.pose.orientation.w = ori_goal[3]
marker.scale.x = 0.0254
marker.scale.y = 0.0254
marker.scale.z = 0.0254
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
#only if using a MESH_RESOURCE marker type:
marker.mesh_resource = "package://hrl_base_selection/models/ADA_Wheelchair.dae"
self.vis_pub.publish( marker )
v = self.robot.GetActiveDOFValues()
for name in self.joint_names:
v[self.robot.GetJoint(name).GetDOFIndex()] = self.joint_angles[self.joint_names.index(name)]
self.robot.SetActiveDOFValues(v)
goal_angle = 0#m.pi/2
self.goal = np.matrix([[ m.cos(goal_angle), -m.sin(goal_angle), 0., .5],
[ m.sin(goal_angle), m.cos(goal_angle), 0., 0],
[ 0., 0., 1., 1.2],
[ 0., 0., 0., 1.]])
#self.goal = copy.copy(self.pr2_B_wc)
#self.goal[0,3]=self.goal[0,3]-.2
#self.goal[1,3]=self.goal[1,3]-.3
#self.goal[2,3]= 1.3
print 'goal is: \n', self.goal
self.goal_B_gripper = np.matrix([[ 0., 0., 1., 0.1],
[ 0., 1., 0., 0.],
[ -1., 0., 0., 0.],
[ 0., 0., 0., 1.]])
self.pr2_B_goal = self.goal*self.goal_B_gripper
self.sol = self.manip.FindIKSolution(np.array(self.pr2_B_goal), op.IkFilterOptions.CheckEnvCollisions)
if self.sol is None:
self.pub_feedback("Failed to find a good arm configuration for reaching.")
return None
rospy.loginfo("[%s] Got an IK solution: %s" % (rospy.get_name(), self.sol))
self.pub_feedback("Found a good arm configuration for reaching.")
self.pub_feedback("Looking for path for arm to goal.")
traj = None
try:
#self.res = self.manipprob.MoveToHandPosition(matrices=[np.array(self.pr2_B_goal)],seedik=10) # call motion planner with goal joint angles
self.traj=self.manipprob.MoveManipulator(goal=self.sol,outputtrajobj=True)
self.pub_feedback("Found a path to reach to the goal.")
except:
self.traj = None
self.pub_feedback("Could not find a path to reach to the goal")
return None
tmp_traj = tmp_vel = tmp_acc = [] #np.zeros([self.traj.GetNumWaypoints(),7])
trajectory = JointTrajectory()
for i in xrange(self.traj.GetNumWaypoints()):
point = JointTrajectoryPoint()
temp = []
for j in xrange(7):
temp.append(float(self.traj.GetWaypoint(i)[j]))
point.positions = temp
#point.positions.append(temp)
#point.accelerations.append([0.,0.,0.,0.,0.,0.,0.])
#point.velocities.append([0.,0.,0.,0.,0.,0.,0.])
trajectory.points.append(point)
#tmp_traj.append(temp)
#tmp_traj.append(list(self.traj.GetWaypoint(i)))
#tmp_vel.append([0.,0.,0.,0.,0.,0.,0.])
#tmp_acc.append([0.,0.,0.,0.,0.,0.,0.])
#print 'tmp_traj is: \n', tmp_traj
#for j in xrange(7):
#tmp_traj[i,j] = float(self.traj.GetWaypoint(i)[j])
#trajectory = JointTrajectory()
#point = JointTrajectoryPoint()
#point.positions.append(tmp_traj)
#point.velocities.append(tmp_vel)
#point.accelerations.append(tmp_acc)
#point.velocities=[[0.,0.,0.,0.,0.,0.,0.]]
#point.accelerations=[[0.,0.,0.,0.,0.,0.,0.]]
trajectory.joint_names = ['l_upper_arm_roll_joint',
'l_shoulder_pan_joint',
'l_shoulder_lift_joint',
'l_forearm_roll_joint',
'l_elbow_flex_joint',
'l_wrist_flex_joint',
'l_wrist_roll_joint']
#trajectory.points.append(point)
#self.mpc_weights_pub.publish(self.weights)
self.moving=True
self.goal_traj_pub.publish(trajectory)
self.pub_feedback("Reaching to location")
import roslib; roslib.load_manifest('hai_sandbox')
import rospy
from geometry_msgs.msg import PoseStamped
def ros_to_dict(msg):
d = {}
for f in msg.__slots__:
val = eval('msg.%s' % f)
methods = dir(val)
if 'to_time' in methods:
val = eval('val.to_time()')
elif '__slots__' in methods:
val = ros_to_dict(val)
d[f] = val
return d
a = PoseStamped()
print ros_to_dict(a)
def calcPose(self, distances, header):
# For the header values of the PoseStamped message, the time stamp will
# be taken from the header sent from the LaserScan message from the
# Arduino (LS_header)
# Determine which sensors are maxed out
maxed_out = []
rows_to_delete = [
] # indices for rows to delete from data to find linear fit
for i in range(len(distances)):
if (distances[i] >= self.threshold_max):
maxed_out.append(1)
rows_to_delete.append(i)
else:
maxed_out.append(0)
# If all sensors are maxed out, theta_p = 0deg
if not (sum(maxed_out) == 6):
# Removed maxed out data
spacing = 2
y = np.array([
distances[0], distances[1], distances[2], distances[3],
distances[4], distances[5]
])
#x = np.array([5*spacing,4*spacing,3*spacing,2*spacing,1*spacing,0*spacing])
x = np.array([
2.5 * spacing, 1.5 * spacing, 0.5 * spacing, -0.5 * spacing,
-1.5 * spacing, -2.5 * spacing
])
y = np.delete(y, rows_to_delete)
x = np.delete(x, rows_to_delete)
# Solve least squares for linear best fit parameters
# y = A*p, A = [x, 1], p = [m, b], whwere line is y = m*x + b
A = np.vstack([x, np.ones(len(x))]).T
m, b = np.linalg.lstsq(A, y)[0]
# Use slope of line (m) to determine angle
theta_p = np.arctan(m) + (np.pi / 2)
theta_p = (180. / np.pi) * theta_p
# Set number of angles to use
num_angles = 5
num_divisions = num_angles - 1
# Create angle divisions from 0 to 180 deg
delta_angle = 180. / num_divisions
angles = np.array([0])
for i in range(1, num_divisions):
angles = np.append(angles, i * delta_angle)
angles = np.append(angles, 180)
# Find closest angle and return it
angle_error = np.abs(angles - theta_p)
theta_p = angles[angle_error.argmin()]
# Convert to range 90 <-> -90 deg
theta_p = -(theta_p - 90)
# Find average distance after removing maxed sensor readings
avg_distance = np.mean(y)
num_sensors = 6 - sum(maxed_out)
# If sensor 1 is maxed out, it means the person is to the left of
# robot, so the robot should rotate counter-clockwise (+1 direction)
if (maxed_out[0]):
rotation_direction = 1
elif (maxed_out[5]):
rotation_direction = -1
else:
rotation_direction = 0
else:
theta_p = 0
avg_distance = self.threshold_max
num_sensors = 0
rotation_direction = 0
x = 0
# Generate pose message
pose = PoseStamped()
pose.header.seq = self.seq
self.seq += 1
pose.header.stamp = header.stamp
pose.header.frame_id = 'IR_Pose'
pose.pose.position.z = avg_distance
pose.pose.position.x = np.mean(x)
k = [0, 1, 0] # axis of rotation, Y-axis
# Quaternion scalar value
q0 = np.cos(theta_p / 2.0)
# Quaternion vector values
q1 = np.sin(theta_p / 2.0) * k[0]
q2 = np.sin(theta_p / 2.0) * k[1]
q3 = np.sin(theta_p / 2.0) * k[2]
pose.pose.orientation.x = q1
pose.pose.orientation.y = q2
pose.pose.orientation.z = q3
pose.pose.orientation.w = q0
return theta_p, avg_distance, num_sensors, rotation_direction, pose
def getStampedPoseMsg(self, pose: Pose):
msg = PoseStamped()
msg.header.frame_id = 'map'
msg.pose = pose
return msg
def talker(filename,
jointTopicName='JointPosition',
poseTopicName='PoseStampedRelative'):
pub = rospy.Publisher('jointAnglesFromFile/' + jointTopicName,
JointPosition,
queue_size=10)
pose_pub = rospy.Publisher('poseFromFile/' + poseTopicName,
PoseStamped,
queue_size=10)
time_pub = rospy.Publisher('iiwa/poseFromFile/receiveTime',
Time,
queue_size=1)
rospy.init_node('jointPosPublisher', anonymous=True)
time.sleep(0.5)
rate = rospy.Rate(SAMPLE_RATE)
reader = csv.reader(open(filename))
reader_list = list(reader)
for i in range(0, NUM_OF_ITERATIONS):
if i % 2 == 0:
for line in reader_list:
if rospy.is_shutdown(): break
values = [float(x) for x in line]
receiveTime = Time()
receiveTime.data = rospy.Time.from_sec(time.time())
#rospy.loginfo(value)
joints = JointPosition()
joints.header.frame_id = "/base_link"
joints.header.stamp = rospy.Time.now()
joints.position.a1 = values[0]
joints.position.a2 = values[1]
joints.position.a3 = values[2]
joints.position.a4 = values[3]
joints.position.a5 = values[4]
joints.position.a6 = values[5]
joints.position.a7 = values[6]
pose = PoseStamped()
pose.header.frame_id = "/world"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = values[7]
pose.pose.position.y = values[8]
pose.pose.position.z = values[9]
pose.pose.orientation.x = values[10]
pose.pose.orientation.y = values[11]
pose.pose.orientation.z = values[12]
pose.pose.orientation.w = values[13]
pub.publish(joints)
pose_pub.publish(pose)
time_pub.publish(receiveTime)
rate.sleep()
else:
for line in reversed(reader_list):
if rospy.is_shutdown(): break
values = [float(x) for x in line]
#rospy.loginfo(value)
joints = JointPosition()
joints.header.frame_id = "/base_link"
joints.header.stamp = rospy.Time.now()
joints.position.a1 = values[0]
joints.position.a2 = values[1]
joints.position.a3 = values[2]
joints.position.a4 = values[3]
joints.position.a5 = values[4]
joints.position.a6 = values[5]
joints.position.a7 = values[6]
pose = PoseStamped()
pose.header.frame_id = "/world"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = values[7]
pose.pose.position.y = values[8]
pose.pose.position.z = values[9]
pose.pose.orientation.x = values[10]
pose.pose.orientation.y = values[11]
pose.pose.orientation.z = values[12]
pose.pose.orientation.w = values[13]
pub.publish(joints)
pose_pub.publish(pose)
rate.sleep()
time.sleep(0.5)
group = moveit_commander.MoveGroupCommander("arm")
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
base_table_id = 'base_table'
# Remove leftover objects from a previous run
scene.remove_world_object(base_table_id)
# Set the dimensions of the scene objects [l, w, h]
base_table_size = [3, 3, 0.01]
# Add a base table to the scene
base_table_pose = PoseStamped()
base_table_pose.header.frame_id = REFERENCE_FRAME
base_table_pose.pose.position.x = 0.0
base_table_pose.pose.position.y = 0.0
#base_table_pose.pose.position.z = TABLE_GROUND - base_table_size[2] / 2.0
base_table_pose.pose.position.z = 0.7
base_table_pose.pose.orientation.w = 1.0
scene.add_box(base_table_id, base_table_pose, base_table_size)
# Give the scene a chance to catch up
rospy.sleep(2)
group.set_max_velocity_scaling_factor(0.5)
while not rospy.core.is_shutdown():
group.set_random_target()
group.go(wait=True)
import sys
import copy
import rospy
from mav_msgs.msg import RollPitchYawrateThrust
from mavros_msgs.msg import AttitudeTarget
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Quaternion
import tf_conversions
from armf import armtakeoff
rospy.init_node('voxboxTOmavros',anonymous=True)
msg = RollPitchYawrateThrust()
setpoint = PoseStamped()
setpoint.pose.position.x = 10
setpoint.pose.position.y = 10
setpoint.pose.position.z = 20
setpoint.header.frame_id = 'map'
setpoint.header.stamp= rospy.Time.now()
def callback(data):
global msg
msg = data
main1()
def main():
"""
Main Script
"""
#===================================================
# Code to add gripper
#rospy.init_node('gripper_test')
#Set up the right gripper
right_gripper = robot_gripper.Gripper('right_gripper')
#Calibrate the gripper (other commands won't work unless you do this first)
# print('Calibrating...')
# right_gripper.calibrate()
# rospy.sleep(2.0)
#Close the right gripper to hold publisher
# print('Closing...')
# right_gripper.close()
# rospy.sleep(1.0)
#===================================================
# Make sure that you've looked at and understand path_planner.py before starting
planner = PathPlanner("right_arm")
Kp = 0.1 * np.array([0.3, 2, 1, 1.5, 2, 2, 3]) # Stolen from 106B Students
Kd = 0.01 * np.array([2, 1, 2, 0.5, 0.5, 0.5, 0.5
]) # Stolen from 106B Students
Ki = 0.01 * np.array([1, 1, 1, 1, 1, 1, 1]) # Untuned
Kw = np.array([0.9, 0.9, 0.9, 0.9, 0.9, 0.9, 0.9]) # Untuned
limb = intera_interface.Limb("right")
control = Controller(Kp, Kd, Ki, Kw, limb)
##
## Add the obstacle to the planning scene here
##
#Tower
#TODO: make wrt to sawyer (currently wrt to ar tag)
X = 0.075
Y = 0.075
Z = 0.045
Xp = 0.0
Yp = -0.0425
Zp = 0.0225
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
# pose = PoseStamped()
# pose.header.stamp = rospy.Time.now()
# #TODO: is this the right frame name?
# pose.header.frame_id = "ar_marker_1"
# pose.pose.position.x = Xp
# pose.pose.position.y = Yp
# pose.pose.position.z = Zp
# pose.pose.orientation.x = Xpa
# pose.pose.orientation.y = Ypa
# pose.pose.orientation.z = Zpa
# pose.pose.orientation.w = Wpa
# planner.add_box_obstacle([X,Y,Z], "tower", pose)
#Table (currently wrt ar tag)
X = 1
Y = 1
Z = .005
Xp = 0
Yp = 0
Zp = 0
Xpa = 0.00
Ypa = 0.00
Zpa = 0.00
Wpa = 1.00
pose = PoseStamped()
pose.header.stamp = rospy.Time.now()
#TODO: Is this the correct frame name?
pose.header.frame_id = "ar_marker_1"
pose.pose.position.x = Xp
pose.pose.position.y = Yp
pose.pose.position.z = Zp
pose.pose.orientation.x = Xpa
pose.pose.orientation.y = Ypa
pose.pose.orientation.z = Zpa
pose.pose.orientation.w = Wpa
planner.add_box_obstacle([X, Y, Z], "table", pose)
# #Create a path constraint for the arm
# #UNCOMMENT FOR THE ORIENTATION CONSTRAINTS PART
orien_const = OrientationConstraint()
orien_const.link_name = "right_gripper_tip"
#changed from "base" frame_id
orien_const.header.frame_id = "ar_marker_1"
orien_const.orientation.w = 1
orien_const.absolute_x_axis_tolerance = 0.001
orien_const.absolute_y_axis_tolerance = 0.001
orien_const.absolute_z_axis_tolerance = 0.001
orien_const.weight = 20.0
rospy.sleep(1.0)
while not rospy.is_shutdown():
while not rospy.is_shutdown():
try:
#currently wrt ar tag
goal_1 = PoseStamped()
goal_1.header.frame_id = "ar_marker_1"
#x, y, and z position
goal_1.pose.position.x = 0.0
goal_1.pose.position.y = -0.0425
goal_1.pose.position.z = 0.0225
#Orientation as a quaternion
goal_1.pose.orientation.x = 0
goal_1.pose.orientation.y = np.pi / 2
goal_1.pose.orientation.z = 0
goal_1.pose.orientation.w = 0
plan = planner.plan_to_pose(goal_1, list())
# if not planner.execute_plan(plan):
if not control.execute_path(plan):
raise Exception("Execution failed")
except Exception as e:
print e
else:
break
def home(self): msg = PoseStamped() self.go_to(msg)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.04
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.02, 0.01, 0.12]
target_x = 0.135
#target_y = -0.32
target_y = -0.285290879999
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.25
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = target_x
target_pose.pose.position.y = target_y
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table blue and the boxes orange
self.setColor(table_id, 0, 0, 0.8, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def main():
rospy.init_node('coke_can_node')
wait_for_time()
# controls of Fetch
##### TODO: Uncomment these if nessecary
head = fetch_api.Head()
arm_joints = fetch_api.ArmJoints()
arm = fetch_api.Arm()
# torso = fetch_api.Torso()
fetch_gripper = fetch_api.Gripper()
move_base_client = actionlib.SimpleActionClient('move_base',
MoveBaseAction)
move_base_client.wait_for_server()
rospy.sleep(0.5)
print "Ready to move"
########## TODO: Uncomment this
# shutdown handler
# def shutdown():
# arm.cancel_all_goals()
# rospy.on_shutdown(shutdown)
# move base to the position near coke can
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 4.50320185696
goal.target_pose.pose.position.y = -4.2
goal.target_pose.pose.orientation.z = -0.554336505677
goal.target_pose.pose.orientation.w = 0.832292639926
move_base_client.send_goal(goal)
wait = move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# move_base_client.get_result()
print "Ready to pick up the coke can!"
# move head
head.pan_tilt(0, 0.7)
print "Head moved!"
##### TODO: write code for moving the arm to pick up the coke can
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.pose.position.x = 0.8
pose.pose.position.y = 0
pose.pose.position.z = 0.75
pose.pose.orientation.w = 1
# arm.move_to_pose(pose)
# print "arm moved"
### MOVE ARM TO
kwargs = {
'allowed_planning_time': 20,
'execution_timeout': 20,
'num_planning_attempts': 1,
'replan_attempts': 5,
'replan': True,
'orientation_constraint': None
}
error = arm.move_to_pose(pose, **kwargs)
if error is not None:
rospy.logerr('Pose failed: {}'.format(error))
else:
rospy.loginfo('Pose succeeded')
print "Arm moved!"
pose = PoseStamped()
pose.header.frame_id = 'base_link'
pose.pose.position.x = 0.5
pose.pose.position.y = 0
pose.pose.position.z = 0.4
pose.pose.orientation.w = 1
error = arm.move_to_pose(pose, **kwargs)
if error is not None:
rospy.logerr('Pose failed: {}'.format(error))
else:
rospy.loginfo('Pose succeeded')
print "Arm moved!"
#TODO:uncomment
# move base to the position near the shelf
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "map"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = 4.50320185696
goal.target_pose.pose.position.y = -4.2
goal.target_pose.pose.orientation.z = -0.554336505677
goal.target_pose.pose.orientation.w = 0.832292639926
move_base_client.send_goal(goal)
wait = move_base_client.wait_for_result()
if not wait:
rospy.logerr("Action server not available!")
rospy.signal_shutdown("Action server not available!")
else:
# move_base_client.get_result()
print "Ready to place the coke can!"
def main():
rospy.init_node('plug_in')
track_outlet_pose = Tracker('/outlet_detector/pose', PoseStamped)
track_plug_pose = Tracker('/plug_detector/pose', PoseStamped)
print "Waiting for mechanism control"
rospy.wait_for_service('kill_and_spawn_controllers')
kill_and_spawn = rospy.ServiceProxy('kill_and_spawn_controllers',
KillAndSpawnControllers)
if rospy.is_shutdown(): sys.exit(0)
# Bring down all possible controllers, just in case
print "Bringing down old controllers"
kill_and_spawn('<controllers></controllers>', [
'r_arm_pose', 'r_arm_twist', 'r_arm_wrench', CONTROLLER, 'arm_hybrid'
])
###### Finds the outlet
# Waits for an estimate of the outlet pose
print "Waiting for outlet pose..."
outlet_pose = None
msg = None
while not msg:
msg = track_outlet_pose.msg
if rospy.is_shutdown(): return
time.sleep(0.1)
outlet_pose = msg
print "Found outlet"
###### Stages the plug
if True:
# Spawns the pose controller
print "Spawning the pose controller"
pose_config = '''
<controllers>
<controller name="r_arm_wrench" type="CartesianWrenchController" />
<controller name="r_arm_twist" type="CartesianTwistController" />
<controller name="r_arm_pose" type="CartesianPoseController" />
</controllers>'''
#resp = kill_and_spawn(pose_config, [])
resp = kill_and_spawn(
'<controller name="r_arm_wrench" type="CartesianWrenchController" />',
[])
resp = kill_and_spawn(
'<controller name="r_arm_twist" type="CartesianTwistController" />',
[])
resp = kill_and_spawn(
'<controller name="r_arm_pose" type="CartesianPoseController" />',
[])
if len(resp.add_name) < 1 or not resp.add_ok[0]:
raise "Failed to spawn the pose controller"
# Commands the hand to near the outlet
print "Staging the plug"
staging_pose = PoseStamped()
staging_pose.header.frame_id = 'outlet_pose'
#staging_pose.pose.position = xyz(-0.12, 0.0, 0.0)
#staging_pose.pose.orientation = rpy(0,0,0)
staging_pose.pose.position = xyz(-0.08, 0.0, 0.04)
staging_pose.pose.orientation = rpy(0.0, 0.3, -0.1)
pub_pose = rospy.Publisher('/r_arm_pose/command', PoseStamped)
for i in range(20):
staging_pose.header.stamp = rospy.get_rostime()
pub_pose.publish(staging_pose)
time.sleep(0.1)
time.sleep(1)
else:
# trajectory controller is already spawned
# TODO: actually check if the traj controller is up. Spawn if not
p_up = PoseStamped()
p_up.header.frame_id = 'base_link'
p_up.header.stamp = rospy.get_rostime()
p_up.pose.position = xyz(0.19, 0.04, 0.5)
p_up.pose.orientation = Quaternion(-0.19, 0.13, 0.68, 0.68)
p_face = PoseStamped()
p_face.header.frame_id = 'base_link'
p_face.header.stamp = rospy.get_rostime()
p_face.pose.position = xyz(0.33, -0.09, 0.37)
p_face.pose.orientation = Quaternion(-0.04, 0.26, -0.00, 0.96)
p_stage1 = PoseStamped()
p_stage1.header.frame_id = 'outlet_pose'
p_stage1.header.stamp = rospy.get_rostime()
p_stage1.pose.position = xyz(-0.12, 0.0, 0.04)
p_stage1.pose.orientation = rpy(0, 0.3, 0)
p_stage2 = PoseStamped()
p_stage2.header.frame_id = 'outlet_pose'
p_stage2.header.stamp = rospy.get_rostime()
p_stage2.pose.position = xyz(-0.07, 0.0, 0.04)
p_stage2.pose.orientation = rpy(0, 0.3, 0)
try:
print "Waiting for the trajectory controller"
move_arm = rospy.ServiceProxy('cartesian_trajectory_right/move_to',
MoveToPose)
print "Staging the plug"
p_up.header.stamp = rospy.get_rostime()
move_arm(p_up)
p_face.header.stamp = rospy.get_rostime()
move_arm(p_face)
p_stage1.header.stamp = rospy.get_rostime()
move_arm(p_stage1)
p_stage2.header.stamp = rospy.get_rostime()
move_arm(p_stage2)
except rospy.ServiceException, e:
print "move_to service failed: %s" % e
#!/usr/bin/env python
# This code is to make drone move around in a circle.
# Headers
import rospy
import math
from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import PoseStamped
current_pos = PoseStamped()
# Just showing how you can the values by subscribing
def current_pos_callback(position):
global current_pos
current_pos = position
#Starting a node
def circle():
rospy.init_node('make_a_circle', anonymous=True)
publish_velocity = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=20)
vel = TwistStamped()
#Getting input
radius = float(input("Enter radius of the circle "))
speed = float(input("Enter the speed of the robot "))
r = abs(radius)
omega = speed / r
def get_path_position_orientation(pose, my_path, timestamp, sync):
if pose:
# Print some of the pose data to the terminal
data = pose.get_pose_data()
w_r = data.rotation.w
x_r = -data.rotation.z
y_r = data.rotation.x
z_r = -data.rotation.y
pitch = -math.asin(2.0 * (x_r*z_r - w_r*y_r)) * 180.0 / math.pi;
roll = math.atan2(2.0 * (w_r*x_r + y_r*z_r), w_r*w_r - x_r*x_r - y_r*y_r + z_r*z_r) * 180.0 / math.pi
yaw = math.atan2(2.0 * (w_r*z_r + x_r*y_r), w_r*w_r + x_r*x_r - y_r*y_r - z_r*z_r) * 180.0 / math.pi
#create path
pose = PoseStamped()
pose.pose.position.x = -data.translation.z
pose.pose.position.z = data.translation.y
pose.pose.position.y = -data.translation.x
pose.pose.orientation.x = pitch
pose.pose.orientation.z = -yaw
pose.pose.orientation.y = roll
pose.pose.orientation.w = 1
pose.header.frame_id = 'camera'
if sync:
#print("trajectory timestamp: ",timestamp)
t1 = (timestamp / 100000000)
t2 = (t1 - int(t1)) * 100000
#t1 = timestamp / 1000.0
time = rospy.Time(secs=int(t2), nsecs = int((t2 - int(t2))*100))
#print("trajectory time: ",time)
my_path.poses.append(pose)
position = [pose.pose.position.x, pose.pose.position.y, pose.pose.position.z]
orientation = [pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z]
odom = Odometry()
odom.header.frame_id = 'camera'
if sync:
odom.header.stamp = time
odom.pose.pose.position.x = data.translation.x
odom.pose.pose.position.z = data.translation.y
odom.pose.pose.position.y = data.translation.z
odom.pose.pose.orientation.x = -x_r
odom.pose.pose.orientation.z = z_r
odom.pose.pose.orientation.y = y_r
odom.pose.pose.orientation.w = w_r
odom.pose.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()
odom.twist.twist.linear.x = data.velocity.x
odom.twist.twist.linear.z = data.velocity.y
odom.twist.twist.linear.y = -data.velocity.z
odom.twist.twist.angular.x = data.acceleration.x
odom.twist.twist.angular.y = data.acceleration.y
odom.twist.twist.angular.z = data.acceleration.z
odom.twist.covariance = np.diag([1e-1, 1e-1, 1e-1, 1e-2, 1e-2, 1e-2]).ravel()
return my_path, position, orientation, odom
def runTest(og_pose, lights):
## face detection
recline_max = 90 # 50 degrees
recline_min = 0
recline_step = 9
sideways_min = -80 # -45 degrees
sideways_max = 80 # 45 degrees
sideways_step = 16
scaling = 100.0
rad2deg = 180.0/3.14
sleepTime = 1
# ONLY MOVE MODEL FOR PICTURE TAKING???
if(False):
theta_ud = -recline_max/scaling
theta_lr = sideways_max/scaling
moveModel('human', og_pose.position, -recline_max/scaling, 0.0, sideways_min/scaling)
return
## landmark detection
rootFrame = 'world'
cameraNameFrame = 'color_corrected_frame'
tfBuffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tfBuffer)
x = []
y = []
z = []
global images_processed
#wait until images_processed topic has been received once:
while(images_processed < 0):
rospy.sleep(1)
start_images_processed = images_processed
faces_detected = 0
#loop through lighting settings:
dataArrayArray = []
for i in range(0,len(lights)):
chooseLight(lights, i)
print "lighting setting ",
print i
dataArray = []
for up in range(recline_min, recline_max, recline_step):
#theta for up/down angle
theta_ud = -up/scaling
data = []
#save y for graphing
for lr in range(sideways_min, sideways_max, sideways_step):
#empty face_coordinates list:
face_coordinates[:] = []
#for a in face_coordinates:
# face_coordinates.pop()
#theta for left/right angle
theta_lr = lr/scaling
#move the model
moveModel('human', og_pose.position, theta_ud, 0.0, theta_lr)
#give time to detect face
startTime = cv2.getTickCount()
while((cv2.getTickCount()-startTime)/cv2.getTickFrequency() < sleepTime):
rospy.sleep(0.001)
#once we have slept, remove noice
for point in face_coordinates:
if point.z_p1 > 1000:
face_coordinates.pop(face_coordinates.index(point))
#print("removed some noise")
faces_detected = faces_detected + len(face_coordinates)
#save amount of detections (face detections)
data.append(len(face_coordinates))
print "faces detected in this pos: ",
print len(face_coordinates)
#calculate position relative to world of face_cords
for faces in face_coordinates:
target_pose = PoseStamped()
target_pose.pose.position.x = faces.x_p2/1000.0
target_pose.pose.position.y = faces.y_p2/1000.0
target_pose.pose.position.z = faces.z_p2/1000.0
target_pose.header.frame_id = cameraNameFrame
target_pose.header.stamp = rospy.Time.now()
try:
camera_transforms = tfBuffer.lookup_transform(rootFrame, cameraNameFrame, rospy.Time(0))
target_transformed_pose = tf2_geometry_msgs.do_transform_pose(target_pose, camera_transforms)
x.append(target_transformed_pose.pose.position.x)
y.append(target_transformed_pose.pose.position.y)
z.append(target_transformed_pose.pose.position.z)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
rospy.spin()
rospy.loginfo("Failed lookup")
return
dataArray.append(data)
dataArrayArray.append(dataArray)
end_images_processed = images_processed
#calculate final detection rate, on a per frame basis:
total_images_processed = end_images_processed - start_images_processed
detection_rate = float(faces_detected) / float(total_images_processed)
detection_rate = round(detection_rate * 100.0, 2)
print "",
print "images processed: ",
print total_images_processed,
print ", detected faces: ",
print faces_detected,
print ", face detection rate: ",
print detection_rate,
print "%"
## landmark detection results
try:
service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState)
req = GetModelState._request_class()
req.model_name = 'human'
req.relative_entity_name = ''
resp = service(req)
except rospy.ServiceException, e:
print("Service call failed: %s" %e)
import cv2
import numpy as np
import matplotlib.path as mpltPath
from geometry_msgs.msg import PoseStamped, Quaternion, PointStamped, Point
from nav_msgs.msg import OccupancyGrid
from map_msgs.msg import OccupancyGridUpdate
from move_base_msgs.msg import MoveBaseActionResult, MoveBaseActionFeedback
# from matplotlib import pyplot as plt
from scipy.interpolate import interp1d
from tf.transformations import quaternion_from_euler
from visualization_msgs.msg import Marker
cost_update = OccupancyGridUpdate()
result = MoveBaseActionResult()
newPose = PoseStamped()
q = Quaternion()
class jackal_explore:
def __init__(self):
self.costmap = OccupancyGrid()
self.flagm = 0
self.flagg = 0
self.init = 0
self.newcost = []
self.grid = OccupancyGrid()
self.feedback = MoveBaseActionFeedback()
self.prevpnt = -1
self.nextpnt = 0
self.sample = []
def __init__(self, nh):
self._run_bz_controller = False
# vel pub
self._vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
self._vel_msg = TwistStamped()
# acc pub
self._accel_pub = rospy.Publisher('/mavros/setpoint_accel/accel',
Vector3Stamped,
queue_size=10)
self._accel_msg = Vector3Stamped()
# attitude
self._att_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
self._att_msg = AttitudeTarget()
self._att_msg.type_mask = 7
# local raw: send acceleration and yaw
self._acc_yaw_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
self._acc_yaw_msg = PositionTarget()
self._acc_yaw_msg.type_mask = 2048 + 32 + 16 + 8 + 4 + 2 + 1 #+ 512
# local raw: send velocity and yaw
self._vel_yaw_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
self._vel_yaw_msg = PositionTarget()
self._vel_yaw_msg.type_mask = 1 + 2 + 4 + 64 + 128 + 256 + 2048
# path bezier triplet send
self._bezier_triplet_pub = rospy.Publisher('/mavros/avoidance_triplet',
AvoidanceTriplet,
queue_size=10)
self._bezier_triplet_msg = AvoidanceTriplet()
self._bezier_duration = 1.0
# initlaize publisher for visualization
self._pub_visualize = pub_bezier.pub_bezier()
# dt
self._dt = 0.0
# call back variables
self._pid_coeff = pidCoeff()
#print self._pid_coeff
# pid tuning parameters with first callback
Server(PIDConfig, self._pidcallback)
self._ctr = controller.controller(self._pid_coeff, 9.91)
### subscriber ###
# state subscriber
self._rate_state = rospy.Rate(RATE_STATE)
self._current_state = State()
rospy.Subscriber('/mavros/state', State, self._current_state_cb)
# subscriber,
self._local_pose = PoseStamped()
self._local_pose.pose.position = cf.p_numpy_to_ros([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._local_pose_cb)
self._local_vel = TwistStamped()
self._local_vel.twist.linear = cf.p_numpy_to_ros([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped,
self._local_vel_cb)
self._bezier_pt = []
'''self._bezier_pt[0] = cf.p_numpy_to_ros([0.0,0.0,0.0])
self._bezier_pt[1] = cf.p_numpy_to_ros([0.0,0.0,0.0])
self._bezier_pt[2] = cf.p_numpy_to_ros([0.0,0.0,0.0]'''
#self._bezier_duration = 1.0
rospy.Subscriber('/path/bezier_pt', Path, self._bezier_cb)
rospy.Subscriber('/path/three_point_message', ThreePointMsg,
self._three_point_msg_cb)
self._linear_acc = Vector3()
self._linear_acc = cf.p_numpy_to_ros_vector([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/imu/data', Imu, self._imu_cb)
def callback(data):
global triggered_time
if triggered_time >= MAX_TIMES + QUEUE_SIZE:
average_error = sum(totalError) / MAX_TIMES
print "Average ERROR: " + str(average_error)
return
else:
triggered_time += 1
currentFrame = data.frameID
for pose in data.poses:
pedID = pose.pedID
if pedsQueue.has_key(pedID):
queue = pedsQueue[pedID]
if len(queue) == QUEUE_SIZE:
queue.pop(0)
queue.append((pose.frameID, pose.x, pose.y))
# queue.sort()
else:
queue = [(pose.frameID, pose.x, pose.y)]
pedsQueue[pedID] = queue
for pedID in pedsQueue:
queue = pedsQueue[pedID]
if currentFrame - queue[-1][0] >= 30:
del pedsQueue[pedID]
continue
# frames = [data for data in range(observation_length+prediction_length,0,-1)]
# xs = [data[1] for data in queue]
# ys = [data[2] for data in queue]
# x_pred = np.zeros(prediction_length)
# y_pred = np.zeros(prediction_length)
# p_x = np.polyfit(frames[:], xs[:], polyfit_degree)
# p_y = np.polyfit(frames[:], ys[:], polyfit_degree)
# p_x = np.poly1d(p_x)
# p_y = np.poly1d(p_y)
# startingFrame = frames[0]
# for i in range(prediction_length):
# x_pred[i] = p_x(startingFrame+i+1)
# y_pred[i] = p_y(startingFrame+i+1)
# validate model
total_length = len(queue)
if total_length < QUEUE_SIZE:
continue
observation_length = total_length - prediction_length
frames = [frame for frame in range(observation_length)]
xs = [data[1] for data in queue]
ys = [data[2] for data in queue]
x_pred = np.zeros(prediction_length)
y_pred = np.zeros(prediction_length)
p_x = np.polyfit(frames[:observation_length], xs[:observation_length],
polyfit_degree)
p_y = np.polyfit(frames[:observation_length], ys[:observation_length],
polyfit_degree)
p_x = np.poly1d(p_x)
p_y = np.poly1d(p_y)
shape_error = (prediction_length, 2)
error = np.zeros(shape_error)
for i in range(prediction_length):
x_pred[i] = p_x(observation_length + i)
y_pred[i] = p_y(observation_length + i)
error[i][0] = x_pred[i] - xs[i + observation_length]
error[i][1] = y_pred[i] - ys[i + observation_length]
totalError.append(
math.sqrt(error[prediction_length - 1][0]**2 +
error[prediction_length - 1][1]**2))
print totalError[-1]
pose_list_pred = list()
my_path_pred = Path()
my_path_pred.header.frame_id = 'velodyne'
pose_list_true = list()
my_path_true = Path()
my_path_true.header.frame_id = 'velodyne'
for i in range(len(x_pred)):
pose = PoseStamped()
loc = Pose()
loc.position.x = x_pred[i]
loc.position.y = y_pred[i]
pose.pose = loc
# pose.header.frame_id = '/odom'
# pose.header.stamp = rospy.Time.now()
pose_list_pred.append(pose)
my_path_pred.poses.append(pose)
for i in range(len(xs)):
pose = PoseStamped()
loc = Pose()
loc.position.x = xs[i]
loc.position.y = ys[i]
pose.pose = loc
# pose.header.frame_id = '/odom'
# pose.header.stamp = rospy.Time.now()
pose_list_true.append(pose)
my_path_true.poses.append(pose)
print '-----------------------------------'
# print y_pred
# print ys[observation_length:]
# print error
path_pub.publish(my_path_true)
predicted_path_validation_pub.publish(my_path_pred)
break
def process_traffic_lights(self):
"""Finds closest visible traffic light, if one exists, and determines its
location and color
Returns:
int: index of waypoint closes to the upcoming stop line for a traffic light (-1 if none exists)
int: ID of traffic light color (specified in styx_msgs/TrafficLight)
"""
light = None
# List of positions that correspond to the line to stop in front of for a given intersection
stop_line_positions = self.config['stop_line_positions']
if (self.pose):
car_position = self.get_closest_waypoint(self.pose.pose)
# TODO find the closest visible traffic light (if one exists)
min_distance = sys.maxsize
light_wp_idx = None
# update waypoint position closest to the car_position
if (self.waypoints == None) or (car_position == None):
return -1, TrafficLight.UNKNOWN
else:
car_pose = self.waypoints.waypoints[
car_position].pose.pose.position
# Find the nearest traffic lights
num_light_idx = len(self.lights)
for i in range(num_light_idx):
lt_pos = self.lights[i].pose.pose.position
dist = self.euclidean_distance(car_pose.x, car_pose.y, car_pose.z,
lt_pos.x, lt_pos.y, lt_pos.z)
if dist < min_distance:
light_wp_idx = i
min_distance = dist
# rospy.loginfo("car_position: %d", car_position)
stop_wp_idx = None
min_dist = sys.maxsize
close_light_idx = self.get_closest_waypoint(
self.lights[light_wp_idx].pose.pose)
if ((light_wp_idx is not None) and (close_light_idx >
(car_position + 1))):
light = self.lights[light_wp_idx].pose.pose.position
state = self.get_light_state()
# find the stop line waypoint to closest traffic light
for i in range(0, len(stop_line_positions)):
stop_line_pos = PoseStamped()
stop_line_pos.pose.position.x = stop_line_positions[i][0]
stop_line_pos.pose.position.y = stop_line_positions[i][1]
stop_line_pos.pose.position.z = 0
closest_wp_idx = self.get_closest_waypoint(stop_line_pos.pose)
stop_lt_pos = self.waypoints.waypoints[
closest_wp_idx].pose.pose.position
dist = self.euclidean_distance(stop_lt_pos.x, stop_lt_pos.y,
stop_lt_pos.z, light.x, light.y,
0)
if (dist < min_dist) and (
closest_wp_idx >
(car_position + 1) % len(self.waypoints.waypoints)):
stop_wp_idx = closest_wp_idx
min_dist = dist
if stop_wp_idx is not None:
# rospy.loginfo("Traffic distance: %d %d %d", min_dist, stop_wp_idx, car_position)
# only update traffic light if min distance is within distance limit
stop_line_pos = self.waypoints.waypoints[
stop_wp_idx].pose.pose.position
self.tl_min_distance = self.euclidean_distance(
car_pose.x, car_pose.y, car_pose.z, stop_line_pos.x,
stop_line_pos.y, stop_line_pos.z)
if (self.tl_min_distance < DISTANCE_LIMIT):
if state == TrafficLight.RED:
state_str = "RED"
elif state == TrafficLight.YELLOW:
state_str = "YELLOW"
else:
state_str = "GREEN"
rospy.logdebug(
"curr_wp_idx = %d, stop_wp_idx = %d, state = %s",
car_position, stop_wp_idx, state_str)
return stop_wp_idx, state
else:
return -1, TrafficLight.UNKNOWN
else:
return -1, TrafficLight.UNKNOWN
else:
return -1, TrafficLight.UNKNOWN
def callback_gps(gps):
global x_charge
global y_charge
global z_charge
global approx_curr
global battery_percentage
global stationary_drain
global default_velocity
global old_location_x
global old_location_y
global old_location_z
time_begin = rospy.Time(720)
time_now = rospy.get_rostime()
if time_now < time_begin:
print('not_yet_started')
old_location_x = gps.pose.position.x
old_location_y = gps.pose.position.y
old_location_z = gps.pose.position.z
battery = PoseStamped()
battery_percentage = 1
battery.pose.position.x = battery_percentage
battery_pub.publish(battery)
if time_now > time_begin:
print('started')
new_location_x = gps.pose.position.x
new_location_y = gps.pose.position.y
new_location_z = gps.pose.position.z
percentage_loss = approx_curr * (math.pow(
(new_location_x - old_location_x), 2) + math.pow(
(new_location_y - old_location_y), 2) + math.pow(
(new_location_z - old_location_z),
2)) / default_velocity + stationary_drain
print("percentage lossp", percentage_loss)
battery_percentage = battery_percentage - percentage_loss
charge_diff = (math.pow((new_location_x - x_charge), 2) + math.pow(
(new_location_y - y_charge), 2) + math.pow(
(new_location_z - z_charge), 2))
if battery_percentage < 0.1:
battery_percentage = 0
print("battery drained")
if charge_diff < 0.5:
battery_percentage = battery_percentage + 0.1
if battery_percentage > 100:
battery_percentage = 100
print("fully charged")
battery = PoseStamped()
battery.pose.position.x = battery_percentage
battery_pub.publish(battery)
charging_setpoint = PoseStamped()
charging_setpoint.pose.position.x = 3
charging_setpoint.pose.position.y = 3
charging_setpoint.pose.position.z = 7
charging_setpoint.pose.orientation.z = 1
charging_setpoint.pose.orientation.w = 1
#charging_setpoint.header.frame_id = "map"
ae_pub.publish(charging_setpoint)
old_location_x = new_location_x
old_location_y = new_location_y
old_location_z = new_location_z
print('battery_percentage', battery_percentage)
def run_network(self):
with lock:
if self.im is None:
return
im = self.im.copy()
depth_cv = self.depth.copy()
rgb_frame_id = self.rgb_frame_id
thread_name = threading.current_thread().name
if not thread_name in self.renders:
print(thread_name)
self.renders[thread_name] = YCBRenderer(
width=cfg.TRAIN.SYN_WIDTH,
height=cfg.TRAIN.SYN_HEIGHT,
gpu_id=cfg.gpu_id,
render_marker=False)
self.renders[thread_name].load_objects(
self.dataset.model_mesh_paths_target,
self.dataset.model_texture_paths_target,
self.dataset.model_colors_target)
self.renders[thread_name].set_camera_default()
self.renders[thread_name].set_light_pos([0, 0, 0])
self.renders[thread_name].set_light_color([1, 1, 1])
print self.dataset.model_mesh_paths_target
cfg.renderer = self.renders[thread_name]
# check the posecnn pose
frame_names, frame_lost, rois_est, poses_est = self.query_posecnn_detection(
)
# cannot initialize
if len(self.objects) == 0 and poses_est.shape[0] == 0:
return
# initialization
if len(self.objects) == 0:
self.frame_names = frame_names
self.frame_lost = frame_lost
self.objects = []
for i in range(poses_est.shape[0]):
obj = {
'frame_name': frame_names[i],
'poses': Queue(maxsize=self.queue_size),
'detected': True
}
obj['poses'].put(poses_est[i, :])
self.objects.append(obj)
else:
# match detection and tracking (simple version)
# for each detected objects
flags_detection = np.zeros((len(frame_names), ), dtype=np.int32)
flags_tracking = np.zeros((len(self.frame_names), ),
dtype=np.int32)
for i in range(len(frame_names)):
for j in range(len(self.frame_names)):
if frame_names[i] == self.frame_names[j]:
# data associated
flags_detection[i] = 1
flags_tracking[j] = 1
self.objects[j]['detected'] = True
break
# undetected
index = np.where(flags_tracking == 0)[0]
index_remove = []
for i in range(len(index)):
ind = index[i]
self.frame_lost[ind] += 1
self.objects[ind]['detected'] = False
if self.frame_lost[ind] >= self.num_lost:
index_remove.append(ind)
# remove item
num = len(self.frame_names)
if len(index_remove) > 0:
self.frame_names = [
self.frame_names[i] for i in range(num)
if i not in index_remove
]
self.frame_lost = [
self.frame_lost[i] for i in range(num)
if i not in index_remove
]
self.objects = [
self.objects[i] for i in range(num)
if i not in index_remove
]
# add new object to track
ind = np.where(flags_detection == 0)[0]
if len(ind) > 0:
for i in range(len(ind)):
self.frame_names.append(frame_names[ind[i]])
self.frame_lost.append(0)
obj = {
'frame_name': frame_names[i],
'poses': Queue(maxsize=self.queue_size),
'detected': True
}
obj['poses'].put(poses_est[ind[i], :])
self.objects.append(obj)
if len(self.objects) == 0:
return
# run network
poses, flags = self.average_poses()
# only refine pose for detected objects
index = np.where(flags == 1)[0]
if len(index) == 0:
return
poses_input = poses[index, :]
im_pose_color, pose_result = test_image(self.net, self.dataset, im,
depth_cv, poses_input,
self.test_data)
pose_msg = self.cv_bridge.cv2_to_imgmsg(im_pose_color)
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = rgb_frame_id
pose_msg.encoding = 'rgb8'
self.pose_pub.publish(pose_msg)
points = self.dataset._points_all
intrinsic_matrix = self.dataset._intrinsic_matrix
# add poses to queue
poses = pose_result['poses_est'][-1]
for i in range(poses.shape[0]):
ind = index[i]
if self.objects[ind]['poses'].full():
self.objects[ind]['poses'].get()
self.objects[ind]['poses'].put(poses[i, :])
poses, flags = self.average_poses()
# poses
for i in range(poses.shape[0]):
cls = int(poses[i, 1])
if cls >= 0:
quat = [poses[i, 3], poses[i, 4], poses[i, 5], poses[i, 2]]
name = self.frame_names[i].replace('posecnn', 'deepim')
print self.dataset.classes[cls], name
self.br.sendTransform(poses[i, 6:], quat, rospy.Time.now(),
name, self.target_frame)
# create pose msg
msg = PoseStamped()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = self.target_frame
msg.pose.orientation.x = poses[i, 3]
msg.pose.orientation.y = poses[i, 4]
msg.pose.orientation.z = poses[i, 5]
msg.pose.orientation.w = poses[i, 2]
msg.pose.position.x = poses[i, 6]
msg.pose.position.y = poses[i, 7]
msg.pose.position.z = poses[i, 8]
pub = self.pubs[cls]
pub.publish(msg)
#'''
# reinitialization if necessary
if poses_est.shape[0] > 0:
# extract 3D points
x3d = np.ones((4, points.shape[1]), dtype=np.float32)
x3d[0, :] = points[cls, :, 0]
x3d[1, :] = points[cls, :, 1]
x3d[2, :] = points[cls, :, 2]
# projection 1
RT = np.zeros((3, 4), dtype=np.float32)
RT[:3, :3] = quat2mat(poses[i, 2:6])
RT[:, 3] = poses[i, 6:]
x2d = np.matmul(intrinsic_matrix, np.matmul(RT, x3d))
x = np.divide(x2d[0, :], x2d[2, :])
y = np.divide(x2d[1, :], x2d[2, :])
x1 = np.min(x)
y1 = np.min(y)
x2 = np.max(x)
y2 = np.max(y)
area = (x2 - x1 + 1) * (y2 - y1 + 1)
# posecnn roi
ind = np.where(rois_est[:, 1] == cls)[0]
if len(ind) > 0:
x1_p = rois_est[ind, 2]
y1_p = rois_est[ind, 3]
x2_p = rois_est[ind, 4]
y2_p = rois_est[ind, 5]
area_p = (x2_p - x1_p + 1) * (y2_p - y1_p + 1)
# compute overlap
xx1 = np.maximum(x1, x1_p)
yy1 = np.maximum(y1, y1_p)
xx2 = np.minimum(x2, x2_p)
yy2 = np.minimum(y2, y2_p)
w = np.maximum(0.0, xx2 - xx1 + 1)
h = np.maximum(0.0, yy2 - yy1 + 1)
inter = w * h
overlap = inter / (area + area_p - inter)
max_overlap = np.max(overlap)
max_ind = np.argmax(overlap)
print('overlap with posecnn box %.2f' % (max_overlap))
if max_overlap < 0.4:
self.objects[i]['poses'].queue.clear()
self.objects[i]['poses'].put(
poses_est[ind[max_ind], :].flatten())
print(
'===================================reinitialize======================================='
)
#!/usr/bin/env python
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import rospy
if __name__ == "__main__":
rospy.init_node('path')
pub = rospy.Publisher('/move_base/GlobalPlanner/plan', Path, queue_size=1)
msg = Path()
msg.poses = [PoseStamped() for i in range(100)]
for i in range(100):
msg.poses[i].pose.position.x = i
msg.poses[i].pose.position.y = i * 2
rate = rospy.Rate(1)
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()
rospy.spin()
def ik_service_client(limb = "right", use_advanced_options = False, position_xyz=[0,0,0], \
orientation_xyzw=[0,0,0,0], seed_position=[0.7, 0.4, -1.7, 1.4, -1.1, -1.6, -0.4]):
"""
:param limb: "right" default
:param use_advanced_options: False default
:param position_xyz: cartesian XYZ [0 0 0] default
:param orientation_xyzw: quaternion for identifying rotation of the end
:param seed_position: where to start the IK optimization
:return:
"""
# return value class
class ret:
def __init__(self):
self.result = False
self.angle = []
# initialize an instance of return value
ret = ret()
# call IK service
ns = "ExternalTools/" + limb + "/PositionKinematicsNode/IKService"
iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
ikreq = SolvePositionIKRequest()
hdr = Header(stamp=rospy.Time.now(), frame_id='base')
# target pose
poses = {
'right':
PoseStamped(
header=hdr,
pose=Pose(
position=Point(
x=position_xyz[0],
y=position_xyz[1],
z=position_xyz[2],
),
orientation=Quaternion(
x=orientation_xyzw[0],
y=orientation_xyzw[1],
z=orientation_xyzw[2],
w=orientation_xyzw[3],
),
),
),
}
# Add desired pose for inverse kinematics
ikreq.pose_stamp.append(poses[limb])
# Request inverse kinematics from base to "right_hand" link
ikreq.tip_names.append('right_hand')
if (use_advanced_options):
# Optional Advanced IK parameters
rospy.loginfo("Running Advanced IK Service Client example.")
# The joint seed is where the IK position solver starts its optimization
ikreq.seed_mode = ikreq.SEED_USER
seed = JointState()
seed.name = [
'right_j0', 'right_j1', 'right_j2', 'right_j3', 'right_j4',
'right_j5', 'right_j6'
]
seed.position = seed_position
ikreq.seed_angles.append(seed)
# Once the primary IK task is solved, the solver will then try to bias the
# the joint angles toward the goal joint configuration. The null space is
# the extra degrees of freedom the joints can move without affecting the
# primary IK task.
ikreq.use_nullspace_goal.append(True)
# The nullspace goal can either be the full set or subset of joint angles
goal = JointState()
goal.name = ['right_j1', 'right_j2', 'right_j3']
goal.position = [1, -1, 2]
ikreq.nullspace_goal.append(goal)
# The gain used to bias toward the nullspace goal. Must be [0.0, 1.0]
# If empty, the default gain of 0.4 will be used
ikreq.nullspace_gain.append(0.4)
else:
rospy.loginfo("Running Simple IK Service Client example.")
try:
rospy.wait_for_service(ns, 5.0)
resp = iksvc(ikreq)
except (rospy.ServiceException, rospy.ROSException), e:
rospy.logerr("Service call failed: %s" % (e, ))
return ret
def __init__(self):
#initialize the move_group api
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo', anonymous=True)
#scene=PlanningSceneInterface()
#self.scene_pub=rospy.Publisher('planning_scene',PlanningScene)
#self.colors=dict()
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
arm.set_planning_time(5)
'''
l_tool_size=[0.02,0.02,0.1]
p=PoseStamped()
p.header.frame_id=end_effector_link
p.pose.position.x=-0.0
p.pose.position.y=0.03
p.pose.position.z=0.0
p.pose.orientation.w=1
scene.attach_box(end_effector_link,'l_tool',p,l_tool_size)
r_tool_size=[0.02,0.02,0.1]
p1=PoseStamped()
p1.header.frame_id=end_effector_link
p1.pose.position.x=-0.0
p1.pose.position.y=-0.03
p1.pose.position.z=0.0
p1.pose.orientation.w=1
scene.attach_box(end_effector_link,'r_tool',p1,r_tool_size)
'''
'''
table_id='table'
box1_id='box1'
box2_id='box2'
tool_id='tool'
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(table_id)
scene.remove_attached_object(end_effector_link,tool_id)
rospy.sleep(2)
table_ground=0.36
table_size=[0.5,0.7,0.01]
box1_size=[0.1,0.05,0.03]
box2_size=[0.05,0.05,0.1]
tool_size=[0.2,0.02,0.02]
table_pose=PoseStamped()
table_pose.header.frame_id=reference_frame
table_pose.pose.position.x=0.8
table_pose.pose.position.y=0.0
table_pose.pose.position.z=table_ground+table_size[2]/2.0
table_pose.pose.orientation.w=1.0
scene.add_box(table_id,table_pose,table_size)
box1_pose=PoseStamped()
box1_pose.header.frame_id=reference_frame
box1_pose.pose.position.x=0.6
box1_pose.pose.position.y=-0.2
box1_pose.pose.position.z=table_ground+table_size[2]+box1_size[2]/2.0
box1_pose.pose.orientation.w=1.0
scene.add_box(box1_id,box1_pose,box1_size)
box2_pose=PoseStamped()
box2_pose.header.frame_id=reference_frame
box2_pose.pose.position.x=0.6
box2_pose.pose.position.y=0.0
box2_pose.pose.position.z=table_ground+table_size[2]+box2_size[2]/2.0
box2_pose.pose.orientation.w=1.0
scene.add_box(box2_id,box2_pose,box2_size)
p=PoseStamped()
p.header.frame_id=end_effector_link
p.pose.position.x=-0.06
p.pose.position.y=0.0
p.pose.position.z=0.0
p.pose.orientation.w=1
scene.attach_box(end_effector_link,'tool',p,tool_size)
'''
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(5)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_footprint'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = -0.09
#target_pose.pose.position.z=table_pose.pose.position.z+table_size[2]+0.15
target_pose.pose.position.z = 0.7
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
#arm.go()
traj = arm.plan()
arm.execute(traj)
rospy.sleep(5)
arm.shift_pose_target(1, -0.1, end_effector_link)
arm.go()
arm.shift_pose_target(3, -1.57, end_effector_link)
arm.go()
rospy.sleep(5)
saved_target_pose = arm.get_current_pose(end_effector_link)
arm.set_named_target("initial_arm2")
arm.go()
rospy.sleep(2)
arm.set_pose_target(saved_target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
arm.set_named_target("initial_arm1")
arm.go()
rospy.sleep(2)
#traj=arm.plan()
#arm.execute(traj)
#rospy.sleep(1)
#positions= [0.113, 0.548, 0.767, 0.505, 0, -0.168, -0.197]
#arm.set_joint_value_target(positions)
#arm.go()
#rospy.sleep(1)
#scene.remove_attached_object(end_effector_link,tool_id)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def control():
# sp = np.load("xy_sin.npy")
state_sub = rospy.Subscriber("/mavros/state",State,state_cb,queue_size=10)
rospy.wait_for_service('mavros/cmd/arming')
arming_client = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
rospy.wait_for_service('mavros/set_mode')
set_mode_client = rospy.ServiceProxy('mavros/set_mode', SetMode)
acutator_control_pub = rospy.Publisher("/mavros/actuator_control",ActuatorControl,queue_size=10)
local_pos_pub = rospy.Publisher("/mavros/setpoint_position/local",PoseStamped,queue_size=10)
mocap_pos_pub = rospy.Publisher("/mavros/mocap/pose",PoseStamped,queue_size=10)
imu_sub = rospy.Subscriber("/mavros/imu/data",Imu,imu_cb, queue_size=10)
local_pos_sub = rospy.Subscriber("/mavros/local_position/pose", PoseStamped, lp_cb, queue_size=10)
local_vel_sub = rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, lv_cb, queue_size=10)
act_control_sub = rospy.Subscriber("/mavros/act_control/act_control_pub", ActuatorControl,act_cb,queue_size=10)
rospy.init_node('control',anonymous=True)
rate = rospy.Rate(50.0)
# print("*"*80)
while not rospy.is_shutdown() and not current_state.connected:
rate.sleep()
rospy.loginfo(current_state.connected)
# print("#"*80)
pose = PoseStamped()
offb_set_mode = SetModeRequest()
offb_set_mode.custom_mode = "OFFBOARD"
arm_cmd = CommandBoolRequest()
arm_cmd.value = True
last_request = rospy.Time.now()
i = 0
act = ActuatorControl()
flag1 = False
flag2 = False
prev_imu_data = Imu()
prev_time = rospy.Time.now()
# prev_x = 0
# prev_y = 0
# prev_z = 0
# prev_vx = 0
# prev_vy = 0
# prev_vz = 0
prev_omega_x = 0
prev_omega_y = 0
prev_omega_z = 0
prev_vx = 0
prev_vy = 0
prev_vz = 0
delta_t = 0.02
count = 0
theta = 0.0
# rospy.loginfo("Outside")
while not rospy.is_shutdown():
if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
offb_set_mode_response = set_mode_client(offb_set_mode)
if offb_set_mode_response.mode_sent:
rospy.loginfo("Offboard enabled")
flag1 = True
last_request = rospy.Time.now()
else:
if current_state.armed == False:
# print(current_state.armed)
arm_cmd_response = arming_client(arm_cmd)
if arm_cmd_response.success :
rospy.loginfo("Vehicle armed")
flag2 = True
last_request = rospy.Time.now()
# rospy.loginfo("Inside")
curr_time = rospy.Time.now()
if flag1 and flag2:
x_f.append(float(local_position.pose.position.x))
y_f.append(float(local_position.pose.position.y))
z_f.append(float(local_position.pose.position.z))
vx_f.append(float(local_velocity.twist.linear.x))
vy_f.append(float(local_velocity.twist.linear.y))
vz_f.append(float(local_velocity.twist.linear.z))
# print(local_position.pose.position.y - count)
orientation = [imu_data.orientation.w,imu_data.orientation.x,imu_data.orientation.y ,imu_data.orientation.z]
(roll,pitch, yaw) = quaternion_to_euler_angle(imu_data.orientation.w,imu_data.orientation.x,imu_data.orientation.y ,imu_data.orientation.z)
r_f.append(float(mt.radians(roll)))
p_f.append(float(mt.radians(pitch)))
yaw_f.append(float(mt.radians(yaw)))
# print('roll angle' , roll)
# print('pitch angle' , pitch)
# print('yaw angle' , yaw)
sin_r_f.append(mt.sin(float(mt.radians(roll))))
sin_p_f.append(mt.sin(float(mt.radians(pitch))))
sin_y_f.append(mt.sin(float(mt.radians(yaw))))
cos_r_f.append(mt.cos(float(mt.radians(roll))))
cos_p_f.append(mt.cos(float(mt.radians(pitch))))
cos_y_f.append(mt.cos(float(mt.radians(yaw))))
rs_f.append(float(imu_data.angular_velocity.x))
ps_f.append(float(imu_data.angular_velocity.y))
ys_f.append(float(imu_data.angular_velocity.z))
ix.append(float(ixx))
iy.append(float(iyy))
iz.append(float(izz))
m.append(float(mass))
u0.append(act_controls.controls[0])
u1.append(act_controls.controls[1])
u2.append(act_controls.controls[2])
u3.append(act_controls.controls[3])
pose.pose.position.x = theta
pose.pose.position.z = 6 + 2*mt.cos(theta*PI/6)
pose.pose.position.y = 0
x_des.append(pose.pose.position.x)
y_des.append(pose.pose.position.y)
z_des.append(pose.pose.position.z)
sin_y_des.append(yaw_des)
cos_y_des.append(yaw_des)
w_mag.append(0)
w_x.append(0)
w_y.append(0)
w_z.append(0)
n_t = curr_time - prev_time
#delta_t = float(n_t.nsecs) * 1e-9
a_x.append((float(local_velocity.twist.linear.x) - prev_vx)/delta_t)
a_y.append((float(local_velocity.twist.linear.y) - prev_vy)/delta_t)
a_z.append((float(local_velocity.twist.linear.z) - prev_vz)/delta_t)
ax_f.append(float(imu_data.linear_acceleration.x))
ay_f.append(float(imu_data.linear_acceleration.y))
az_f.append(float(imu_data.linear_acceleration.z))
prev_vx = float(local_velocity.twist.linear.x)
prev_vy = float(local_velocity.twist.linear.y)
prev_vz = float(local_velocity.twist.linear.z)
aplha_x.append((float(imu_data.angular_velocity.x) - prev_omega_x)/delta_t)
aplha_y.append((float(imu_data.angular_velocity.y) - prev_omega_y)/delta_t)
aplha_z.append((float(imu_data.angular_velocity.z) - prev_omega_z)/delta_t)
prev_omega_x = float(imu_data.angular_velocity.x)
prev_omega_y = float(imu_data.angular_velocity.y)
prev_omega_z = float(imu_data.angular_velocity.z)
theta += 1.0/10
count += 1
local_pos_pub.publish(pose)
if(count >= data_points):
break
prev_time = curr_time
rate.sleep()
nn1_yz_input_state = np.array([vx_f,vy_f,vz_f, rs_f,ps_f,ys_f, sin_r_f, sin_p_f,sin_y_f, cos_r_f,cos_p_f,cos_y_f, u3],ndmin=2).transpose()
nn1_yz_grd_truth = np.array([a_x,a_y,a_z],ndmin=2).transpose()
nn2_yz_input_state = np.array([vx_f,vy_f,vz_f, rs_f,ps_f,ys_f, sin_r_f, sin_p_f,sin_y_f, cos_r_f,cos_p_f,cos_y_f, u0, u1, u2],ndmin=2).transpose()
nn2_yz_grd_truth = np.array([aplha_x,aplha_y,aplha_z],ndmin=2).transpose()
print('nn1_yz_input_state :',nn1_yz_input_state.shape)
print('nn1_yz_grd_truth :',nn1_yz_grd_truth.shape)
print('nn2_yz_input_state :',nn2_yz_input_state.shape)
print('nn2_yz_grd_truth :',nn2_yz_grd_truth.shape)
np.save('nn1_x_cos_z_input_state.npy',nn1_yz_input_state)
np.save('nn1_x_cos_z_grd_truth.npy',nn1_yz_grd_truth)
np.save('nn2_x_cos_z_input_state.npy',nn2_yz_input_state)
np.save('nn2_x_cos_z_grd_truth.npy',nn2_yz_grd_truth)
s_x_cos_z = np.array([x_f, y_f, z_f,vx_f,vy_f,vz_f,sin_r_f,sin_p_f,sin_y_f,cos_r_f,cos_p_f,cos_y_f,rs_f,ps_f,ys_f,r_f,p_f,yaw_f],ndmin=2).transpose()
u_x_cos_z = np.array([u0,u1,u2,u3],ndmin=2).transpose()
a_x_cos_z = np.array([ax_f,ay_f,az_f],ndmin=2).transpose()
print('s_x_cos_z :',s_x_cos_z.shape)
print('u_x_cos_z :',u_x_cos_z.shape)
print('a_x_cos_z :',a_x_cos_z.shape)
np.save('s_x_cos_z.npy' ,s_x_cos_z)
np.save('u_x_cos_z.npy' ,u_x_cos_z)
np.save('a_x_cos_z.npy' ,a_x_cos_z)
def __init__(self):
rospy.init_node('wrench_correction', anonymous=False)
rospy.on_shutdown(
self.cleanup) # shutdown function when executing done
self.flag = 0
self.ct = 0
rospy.Subscriber("move_group/status",
GoalStatusArray,
self.callback,
queue_size=1)
rospy.loginfo("Wrench Correction step")
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
self.arm = moveit_commander.MoveGroupCommander("ur5_arm")
# Get the name of the end-effector link
self.end_effector_link = self.arm.get_end_effector_link()
# Initialize Necessary Variables
self.reference_frame = rospy.get_param("~reference_frame",
"/base_link")
# Set the ur5_arm reference frame accordingly
self.arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
#Set the target pose from the input
self.target_pose = PoseStamped()
self.target_pose.header.frame_id = reference_frame
self.target_pose.header.stamp = rospy.Time.now()
self.target_pose.pose.position.x = locations.point.x
self.target_pose.pose.position.y = locations.point.y
self.target_pose.pose.position.z = locations.point.z
# Set the start state to the current state
try:
cjs = rospy.get_param('current_joint_state')
except:
cjs = [0, 0, 0, 0, 0, 0]
jt = RobotState() # Get current state from Robot State
jt.joint_state.header.frame_id = '/base_link'
jt.joint_state.name = [
'front_left_wheel', 'front_right_wheel', 'rear_left_wheel',
'rear_right_wheel', 'ur5_arm_shoulder_pan_joint',
'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint',
'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint',
'ur5_arm_wrist_3_joint', 'left_tip_hinge', 'right_tip_hinge'
]
jt.joint_state.position = [
0, 0, 0, 0, cjs[0], cjs[1], cjs[2], cjs[3], cjs[4], cjs[5], 0, 0
]
self.arm.set_start_state(jt)
# Set the goal pose of the end effector to the stored pose
self.arm.set_pose_target(self.target_pose, self.end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
if traj is not None:
# Execute the planned trajectory
arm.execute(traj)
rospy.loginfo("Moving to wrench location")
# Pause for a second
rospy.sleep(3.0)
# Moving toward the wrench
rospy.loginfo("Approaching wrench")
arm.shift_pose_target(1, 0.05, end_effector_link)
arm.go()
rospy.loginfo("Moving forward 5 cm")
rospy.sleep(3.0)
rospy.loginfo("Approaching successfully")
rospy.loginfo("Successfully moved")
else:
rospy.loginfo("Unable to reach")
#else:
# print usage()
# sys.exit(1)
print "Requesting Base Goal Position"
goal_array, config_array = select_base_client()
base_goals = []
configuration_goals = []
for item in goal_array:
base_goals.append(item)
for item in config_array:
configuration_goals.append(item)
base_goals_list = []
configuration_goals_list = []
for i in xrange(int(len(base_goals)/7)):
psm = PoseStamped()
psm.header.frame_id = '/base_link'
psm.pose.position.x = base_goals[int(0+7*i)]
psm.pose.position.y = base_goals[int(1+7*i)]
psm.pose.position.z = base_goals[int(2+7*i)]
psm.pose.orientation.x = base_goals[int(3+7*i)]
psm.pose.orientation.y = base_goals[int(4+7*i)]
psm.pose.orientation.z = base_goals[int(5+7*i)]
psm.pose.orientation.w = base_goals[int(6+7*i)]
psm.header.frame_id = '/base_link'
base_goals_list.append(copy.copy(psm))
configuration_goals_list.append([configuration_goals[0+3*i], configuration_goals[1+3*i]+9.+10,
configuration_goals[2+3*i]+40.])
publish_to_autobed(configuration_goals_list)
publish_to_zaxis(configuration_goals_list)
rospy.init_node('lab4')
# Global Variables
global new_map_flag
global world_map
global goal_pose
global cost_map
# Global Variables for Navigation
global pose
global odom_tf
global odom_list
global prev_twist
# Initialize Global Variables
pose = PoseStamped()
new_map_flag = 0
world_map = None
start_pose = None
goal_pose = None
cost_map = OccupancyGrid()
# Initialize Navigation GV
prev_twist = Twist()
prev_twist.linear.x = 0
prev_twist.angular.z = 0
odom_list = tf.TransformListener()
odom_tf = tf.TransformBroadcaster()
odom_tf.sendTransform((0, 0, 0), (0, 0, 0, 1), rospy.Time.now(),
"base_footprint", "odom")
