def copDetection():
copName = "deckard"
robberName = "roy"
# Get list of objects and their locations
mapInfo = 'map2.yaml'
objLocations = getObjects(mapInfo)
vertexes = objLocations.values()
vertexKeys = objLocations.keys()
# TODO: GetPlan not working because move base does not provide the service (EITHER FIX IT OR FIND A NEW WAY TO GET DISTANCE)
# tol = .1
# robberName = "roy"
# robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,0,1)))
# destination = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1)))
# print(robLoc)
# print(destination)
# # Get plan from make_plan service
# #rospy.wait_for_service("/" + robberName + "move_base/make_plan")
# try:
# planner = rospy.ServiceProxy("/" + robberName + "/move_base/make_plan", nav_srv.GetPlan)
# plan = planner(robLoc, destination, tol)
# poses = plan.plan.poses
# print(poses)
# except rospy.ServiceException, e:
# print "GetPlan service call failed: %s"%e
# return 0
# rospy.Subscriber("/" + copName + "/", geo_msgs.Pose, getCopLocation)
# rospy.Subscriber("/" + robberName + "/", geo_msgs.Pose, getRobberLocation)
copLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,1,0)))
pastCopLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,1,0)))
robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,1,0)))
test_path = [geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1))),
geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,0,1)))]
test_plans = {"kitchen": nav_msgs.Path(std_msgs.Header(), test_path)}
curCost, curDestination = chooseDestination(test_plans, copLoc, robLoc, pastCopLoc)
print("Minimum Cost: " + str(curCost) + " at " + curDestination)
#move base to curDestination
# state = mover_base.get_state()
# pathFailure = False
# while (state!=3 and pathFailure==False): # SUCCESSFUL
# #wait a second rospy.sleep(1)
# newCost = evaluatePath(test_plans[curDestination])
# if newCost > curCost*2:
# pathFailure = True
# rospy.loginfo("Just Reached " + vertexKeys[i])
# Floyd warshall stuff
mapGrid = np.load('mapGrid.npy')
floydWarshallCosts = np.load('floydWarshallCosts.npy')
evaluateFloydCost(copLoc, robLoc, floydWarshallCosts, mapGrid)
def publishTagMarkers(tags, publisher):
"""Republishes the tag_markers topic"""
SHIFT = 0.05
h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map')
ca = std_msgs.ColorRGBA(r=1, a=1)
cb = std_msgs.ColorRGBA(g=1, a=1)
ct = std_msgs.ColorRGBA(b=1, a=1)
sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5)
sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25)
st = geometry_msgs.Vector3(x=1, y=1, z=1)
ma = visualization_msgs.MarkerArray()
ma.markers.append(
visualization_msgs.Marker(
header=h, id=-1, action=visualization_msgs.Marker.DELETEALL))
for i, t in enumerate(tags):
th = t['th_deg'] * math.pi / 180.
q = tf.transformations.quaternion_from_euler(0, 0, th)
q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t['x'], y=t['y']),
orientation=q),
scale=sa,
color=ca))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 1,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(
x=t['x'] + SHIFT * math.cos(th),
y=t['y'] + SHIFT * math.sin(th)),
orientation=q),
scale=sb,
color=cb))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 2,
type=visualization_msgs.Marker.TEXT_VIEW_FACING,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5),
orientation=q),
scale=st,
color=ct,
text=str(t['id'])))
publisher.publish(ma)
def get_up_stretched_0123wa():
fl_pose = geomsg.Pose()
fl_pose.position.x = 0.35
fl_pose.position.y = 0.35
fl_pose.position.z = -0.7
fl_pose.orientation.x = 0.0
fl_pose.orientation.y = 0.0
fl_pose.orientation.z = 0.0
fl_pose.orientation.w = 1.0
fr_pose = geomsg.Pose()
fr_pose.position.x = 0.35
fr_pose.position.y = -0.35
fr_pose.position.z = -0.7
fr_pose.orientation.x = 0.0
fr_pose.orientation.y = 0.0
fr_pose.orientation.z = 0.0
fr_pose.orientation.w = 1.0
rr_pose = geomsg.Pose()
rr_pose.position.x = -0.35
rr_pose.position.y = -0.35
rr_pose.position.z = -0.7
rr_pose.orientation.x = 0.0
rr_pose.orientation.y = 0.0
rr_pose.orientation.z = 0.0
rr_pose.orientation.w = 1.0
rl_pose = geomsg.Pose()
rl_pose.position.x = -0.35
rl_pose.position.y = 0.35
rl_pose.position.z = -0.7
rl_pose.orientation.x = 0.0
rl_pose.orientation.y = 0.0
rl_pose.orientation.z = 0.0
rl_pose.orientation.w = 1.0
waist = geomsg.Pose()
waist.position.x = 0.0
waist.position.y = 0.0
waist.position.z = 0.95
waist.orientation.x = 0.0
waist.orientation.y = 0.0
waist.orientation.z = 0.0
waist.orientation.w = 1.0
return (fl_pose, fr_pose, rr_pose, rl_pose, waist)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = mil_msgs.MoveToGoal(
header=mil_ros_tools.make_header('/map'),
posetwist=mil_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def execute(self, ud):
start_time = rospy.get_time()
rate = rospy.Rate(2.5)
while not self.preempt_requested() and not rospy.is_shutdown():
segmented_objs = self.segment_srv(
only_surface=True).segmented_objects.objects
if segmented_objs:
table = segmented_objs[0]
pose = geo_msgs.PoseStamped(
table.point_cloud.header,
geo_msgs.Pose(table.center, table.orientation))
pose = TF2().transform_pose(pose, pose.header.frame_id, 'map')
width, length = table.width, table.depth
table_tf = to_transform(pose, 'table_frame')
TF2().publish_transform(table_tf)
if 'table' in ud:
table.name = ud['table'].name
ud['table'] = table
ud['table_pose'] = pose
rospy.loginfo("Detected table of size %.1f x %.1f at %s",
width, length, pose2d2str(pose))
return 'succeeded'
elif self.timeout and rospy.get_time() - start_time > self.timeout:
rospy.logwarn("No table detected after %g seconds",
rospy.get_time() - start_time)
return 'aborted'
try:
rate.sleep()
except rospy.ROSInterruptException:
break
rospy.logwarn(
"Detect table has been preempted after %g seconds (timeout %g)",
rospy.get_time() - start_time, self.timeout)
return 'preempted'
def start_MM(self, mess):
if mess.data == True:
print("MissionManagment Started")
#Intial Pose
camera_initial_pose = geo_msg.Pose()
camera_initial_pose.position.x = 0.2
camera_initial_pose.position.y = -0.4
camera_initial_pose.position.z = 0.4
camera_initial_pose.orientation.x = 0
camera_initial_pose.orientation.y = 1
camera_initial_pose.orientation.z = 0
camera_initial_pose.orientation.w = 0
self.pose_ur_pub.publish(camera_initial_pose)
time.sleep(5)
startEMVS = self.startEMVScall(True)
time.sleep(1)
#Velocity Command
camera_vel_cmd = geo_msg.Twist()
camera_vel_cmd.linear.x = 0.03
camera_vel_cmd.linear.y = -0.006
camera_vel_cmd.linear.z = 0
camera_vel_cmd.angular.x = 0
camera_vel_cmd.angular.y = 0
camera_vel_cmd.angular.z = 0
self.vel_ur_pub.publish(camera_vel_cmd)
def test_pose_init(self):
""" Test if pose class can be initialized as expected. """
# Create from quaternion
p = Point(1, 3, 2)
o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2))
pose = Pose(p, o)
# Create from angle
pose2 = Pose(p, math.pi / 2)
self.assertEqual(pose, pose2)
g_pose = g_msgs.Pose()
g_pose.position = p.to_geometry_msg()
g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_pose), pose)
# Create from geometry_msgs/transform
g_tf = g_msgs.Transform()
g_tf.translation = p.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_tf), pose)
def vel_callback(msg):
global pose_lock, saved_pose
twist = gms.Twist()
twist.linear.x = msg.linear[0]
twist.linear.y = msg.linear[1]
twist.linear.z = msg.linear[2]
twist.angular.x = msg.angular[0]
twist.angular.y = msg.angular[1]
twist.angular.z = msg.angular[2]
twist_pub.publish(twist)
pose_lock.acquire()
ps = cp.copy(saved_pose)
pose_lock.release()
if not ps is None:
translation = ps.position
rotation = ps.orientation
cmd_pose = gms.PoseStamped()
cmd_pose.pose = gms.Pose(
position=translation,
orientation=rotation)
cmd_pose_pub.publish(cmd_pose)
transform = gms.Transform(translation, rotation)
cmd_traj = tms.MultiDOFJointTrajectory()
cmd_traj.points.append(tms.MultiDOFJointTrajectoryPoint())
cmd_traj.points[0].transforms.append(transform)
cmd_traj.points[0].velocities.append(twist)
cmd_traj_pub.publish(cmd_traj)
def test_sd_pose(self):
"""
Initial pose of franka panda_arm group
position:
x: 0.307019570052
y: -5.22132961561e-12
z: 0.590269558277
orientation:
x: 0.923955699469
y: -0.382499497279
z: 1.3249325839e-12
w: 3.20041176635e-12
:return:
"""
ros_pose = GeometryMsg.Pose()
ros_pose.position.x = 0.307019570052
ros_pose.position.y = 0
ros_pose.position.z = 0.590269558277
ros_pose.orientation.x = 0.923955699469
ros_pose.orientation.y = -0.382499497279
ros_pose.orientation.z = 0
ros_pose.orientation.w = 0
pose_mat = common.sd_pose(ros_pose)
re_pose = common.to_ros_pose(pose_mat)
self.assertTrue(common.all_close(ros_pose, re_pose, 0.001))
def getObjects(mapInfo):
with open(mapInfo, 'r') as stream:
try:
yamled = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
# deletes info
del yamled['info']
# Populates dictionary with location names and their poses
objDict = yamled.values()
objLocations = {}
objNames = {}
for item in objDict:
itemName = item['name']
if itemName[0:4] != "wall":
x_loc = item['centroid_x'] + (item['width'] / 2 + .6) * math.cos(
math.radians(item['orientation']))
y_loc = item['centroid_y'] + (item['length'] / 2 + .6) * math.sin(
math.radians(item['orientation']))
quat = tf.transformations.quaternion_from_euler(
0, 0, item['orientation'] - 180)
itemLoc = geo_msgs.PoseStamped(
std_msgs.Header(),
geo_msgs.Pose(
geo_msgs.Point(x_loc, y_loc, 0),
geo_msgs.Quaternion(quat[0], quat[1], quat[2], quat[3])))
objLocations[itemName] = itemLoc
objNames[itemName] = ([item['value']])
return objLocations, objNames
def build_cart_request(pos, quat, arm, initial_state = INITIAL_ROBOT_STATE, planner_id=''):
m = MotionPlanRequest()
m.group_name = "%s_arm" % arm
target_link = "%s_wrist_roll_link" % arm[0]
m.start_state = initial_state
m.planner_id = planner_id
pc = PositionConstraint()
pc.link_name = target_link
pc.header.frame_id = 'odom_combined'
pose = gm.Pose()
pose.position = pos
pc.constraint_region.primitive_poses = [pose]
sphere = SolidPrimitive()
sphere.type = SolidPrimitive.SPHERE
sphere.dimensions = [.01]
pc.constraint_region.primitives = [ sphere ]
oc = OrientationConstraint()
oc.link_name = target_link
oc.header.frame_id = 'odom_combined'
oc.orientation = quat
c = Constraints()
c.position_constraints = [ pc ]
c.orientation_constraints = [ oc ]
m.goal_constraints = [ c ]
m.allowed_planning_time = rospy.Duration(args.max_planning_time)
return m
def make_pose_stamped(position, orientation, frame='/body'):
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
pose=geometry_msgs.Pose(
force=geometry_msgs.Vector3(*position),
orientation=geometry_msgs.Quaternion(*orientation)))
return wrench
def pub_loc(self,publisher): pub = geo.Pose() if len(self.nums)<2: pub.position.x = -10 pub.position.y = -10 pub.position.z = -10 elif len(self.nums)>=5: pub.position.x = 10 pub.position.y = 10 pub.position.z = 10 self.done = True else: nxt = self.next_loc() pub.position.x = nxt[0] pub.position.y = nxt[1] if pub.position.x > 3.5: pub.position.x = 3.5 elif pub.position.x < 0.0: pub.position.x = 0.0 if pub.position.y > 3.3: pub.position.y = 3.3 elif pub.position.y < -2.0: pub.position.y = -2.0 eul = (0.0,0.0,math.atan2(nxt[1]-1, nxt[0]-2)) quat = tft.quaternion_from_euler(0.0,0.0, math.atan2(nxt[1],nxt[0])) pub.orientation.x = quat[0] pub.orientation.y = quat[1] pub.orientation.z = quat[2] pub.orientation.w = quat[3] publisher.publish(pub)
def PoseStamped(x=0, y=0, z=0, phi=0,
roll=0, pitch=0, yaw=0,
frame_id="/map", stamp=None, pointstamped=None):
"""Build a geometry_msgs.msgs.PoseStamped from any number of arguments.
Each value defaults to 0
>>> ps = PoseStamped(yaw=0.5)
>>> ps.pose
position:
x: 0
y: 0
z: 0
orientation:
x: 0.0
y: 0.0
z: 0.247403959255
w: 0.968912421711
"""
if not stamp:
stamp = rospy.get_rostime()
if pointstamped:
return gm.PoseStamped(header=pointstamped.header,
pose=gm.Pose(pointstamped.point, Quaternion()))
elif isinstance(x, number) and isinstance(y, number) and isinstance(z, number):
return gm.PoseStamped(header=Header(frame_id, stamp),
pose=Pose(x, y, z, phi, roll, pitch,yaw))
else:
raise ValueError("Either supply a number for x, y and z or a PointStamped to pointstamped")
def execute(self, userdata):
rospy.loginfo('Executing state CLEAR WORKSPACE')
# CHANGE CONTROLLER TO WHEELED MOTION
robot.load_controller('WheeledMotion')
# CALL ACTION TO GO DOWN AND WAIT FOR RESULT
(l_pose, r_pose) = poses.get_clear_workspace_lr()
waist_delta_pose = geomsg.Pose()
waist_delta_pose.position.z = -0.1
robot.go_to('waist', waist_delta_pose, 6.0, True)
robot.go_to('fl_wheel', l_pose, 6.0)
robot.go_to('fr_wheel', r_pose, 6.0)
robot.wait_for_result('waist')
robot.wait_for_result('fl_wheel')
robot.wait_for_result('fr_wheel')
# BACK TO FIXED-WHEELS CONTROLLER
robot.load_controller('OpenSot')
text = ''
while not (text == 'Y' or text == 'n'):
text = raw_input('Workspace cleared correcly? [Y/n]')
if text == 'Y':
return 'ws_cleared'
else:
return 'failure'
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = sub8_utils.make_header(frame='/world')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(-quaternion[1], -quaternion[2], -quaternion[3], quaternion[0]),
)
twist = geometry.Twist(
linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel)
)
odom_msg = Odometry(
header=header,
child_frame_id='/body',
pose=geometry.PoseWithCovariance(
pose=pose
),
twist=geometry.TwistWithCovariance(
twist=twist
)
)
self.truth_odom_pub.publish(odom_msg)
def pub_target_timer_callback(self):
(xyz, rpy) = planner_instance._target_pose
q = tf.transformations.quaternion_from_euler(*rpy)
msg = geometry_msgs.Pose(geometry_msgs.Point(*xyz),
geometry_msgs.Quaternion(*q))
publisher.publish(msg)
def distance_2d(pose1, pose2=None):
if not pose2:
pose2 = pose1
pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin
return sqrt(
pow(pose2.position.x - pose1.position.x, 2) +
pow(pose2.position.y - pose1.position.y, 2))
def offset_ros_pose(pose, offset):
output = GeometryMsg.Pose()
output.position.x = pose.position.x + offset[0]
output.position.y = pose.position.y + offset[1]
output.position.z = pose.position.z + offset[2]
output.orientation = pose.orientation
return output
def Pose(x=0, y=0, z=0, phi=0, roll=0, pitch=0, yaw=0):
"""
>>> pose = Pose(yaw=0.5)
>>> pose
position:
x: 0
y: 0
z: 0
orientation:
x: 0.0
y: 0.0
z: 0.247403959255
w: 0.968912421711
"""
if phi:
rospy.logerr(
"Please specify yaw instead of phi. Phi will be removed!!!")
z_rotation = phi or yaw #Get the one that is not 0
quat = Quaternion(roll=roll, pitch=pitch, yaw=z_rotation)
pos = Point(x, y, z)
return gm.Pose(pos, quat)
def __init__(self,
pose_topic='/pick_up_parcel/pose',
twist_topic='/pick_up_parcel/cmd_vel',
duration=None):
super(PickUpParcel,
self).__init__(action_name="pick_up_parcel",
action_type=dyno_msgs.QuadrotorPickUpParcelAction,
worker=self.worker,
duration=duration)
self.initial_height = 1.0
self.goal_height = 0.2
self.pose = geometry_msgs.Pose()
self.pose.position.z = self.initial_height
self.twist = geometry_msgs.Twist()
latched = True
queue_size_five = 1
self.publishers = py_trees_ros.utilities.Publishers([
('pose', pose_topic, geometry_msgs.Pose, latched, queue_size_five),
('twist', twist_topic, geometry_msgs.Twist, latched,
queue_size_five)
])
# self.publishers.pose.publish(self.pose)
# self.publishers.twist.publish(self.twist)
self.publishing_timer = rospy.Timer(period=rospy.Duration(0.5),
callback=self.publish,
oneshot=False)
self.start()
def heading(pose1, pose2=None):
""" Heading angle from one pose to another.
Poses are assumed to have the same reference frame. """
if not pose2:
pose2 = pose1
pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin
p1, p2 = __get_naked_poses(pose1, pose2)
return atan2(p2.position.y - p1.position.y, p2.position.x - p1.position.x)
def move_to_pos_quat(group, pos, quat):
pose_goal = gm.Pose(gm.Point(*pos), gm.Quaternion(*quat))
group.set_pose_target(pose_goal)
plan_success = group.go(wait=True)
group.stop()
group.clear_pose_targets()
return plan_success
def geom_pose_from_rt(rt):
msg = geom_msg.Pose()
msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2])
xyzw = rt.quat.to_xyzw()
msg.orientation = geom_msg.Quaternion(x=xyzw[0],
y=xyzw[1],
z=xyzw[2],
w=xyzw[3])
return msg
def set_random_pose(self, center, scale):
'''Set a random position, and for now constant orientation
TODO: Random orientation
'''
pos = center + (np.random.random(3) * scale)
self.set_pose(pose=geometry_msgs.Pose(
position=pos,
orientation=(0.0, 0.0, 0.0, 1.0),
))
def generate_plan():
target_pose = geometry_msg_lib.Pose()
target_pose.orientation.w = 1.0
target_pose.position.x = 0.7
target_pose.position.y = -0.05
target_pose.position.z = 1.1
move_group.set_pose_target(target_pose)
return move_group.plan()
def draw_curve(arr):
pose_array = gmm.PoseArray()
for (x, y, z) in arr:
pose = gmm.Pose()
pose.position = gmm.Point(x, y, z)
pose_array.poses.append(pose)
pose_array.header.frame_id = "/base_footprint"
pose_array.header.stamp = rospy.Time.now()
rviz.draw_curve(pose_array)
def distance_2d(pose1, pose2=None):
""" Euclidean distance between 2D poses; z coordinate is ignored.
Poses are assumed to have the same reference frame. """
if not pose2:
pose2 = pose1
pose1 = geometry_msgs.Pose() # 0, 0, 0 pose, i.e. origin
p1, p2 = __get_naked_poses(pose1, pose2)
return sqrt(
pow(p2.position.x - p1.position.x, 2) +
pow(p2.position.y - p1.position.y, 2))
def to_pose3d(pose, timestamp=rospy.Time(), frame=None):
if isinstance(pose, geometry_msgs.Pose2D):
p = geometry_msgs.Pose(geometry_msgs.Point(pose.x, pose.y, 0.0),
quaternion_msg_from_yaw(pose.theta))
if not frame:
return p
return geometry_msgs.PoseStamped(std_msgs.Header(0, timestamp, frame),
p)
raise rospy.ROSException(
"Input parameter pose is not a geometry_msgs.Pose2D object")
def tup_to_pose(p):
pose = gmsg.Pose()
pose.position.x = p[0][0]
pose.position.y = p[0][1]
pose.position.z = p[0][2]
pose.orientation.x = p[1][0]
pose.orientation.y = p[1][1]
pose.orientation.z = p[1][2]
pose.orientation.w = p[1][3]
return pose
