def __init__(self):
print("HsrArm is actived.")
self.check_mobility = Mobility()
self.mvt = HsrMove()
self.listener = l()
self.utils = Utils()
self.listener.set_topic_and_typMEssage(
"/hsrb/arm_trajectory_controller/state",
JointTrajectoryControllerState)
def __init__(self):
# initialize object
print("HSR_MOVE is actived")
# Set Mobility checker
self.check_mobility = Mobility()
self.interface = GiskardWrapper()
self.pose_stamped = PoseStamped()
self.states_pre_grasp = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": 0.0,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.place_states = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": -1.57,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.primary_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": -0.026,
"arm_roll_joint": -1.57,
"wrist_flex_joint": -1.57,
"wrist_roll_joint": 0.0
}
self.end_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.0,
"arm_roll_joint": 1.57
}
self.perceive_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.70,
"arm_roll_joint": 1.57
}
def _update_links(self):
# setting downstream values, this should change from initalisation
# to just updating through the update function
self.Mob = Mob(matterial=self.cal_dts['matterial'],
author=self.cal_dts['mob_author'],
temp=self.cal_dts['temp'])
self.ni = ni(matterial=self.cal_dts['matterial'],
author=self.cal_dts['nieff_author'],
temp=self.cal_dts['temp'])
self.ion = Ion(matterial=self.cal_dts['matterial'],
author=self.cal_dts['ionis_author'],
temp=self.cal_dts['temp'])
class HsrArm:
"""
This class determinate and give all move joints of hsrs arm
"""
def __init__(self):
print("HsrArm is actived.")
self.check_mobility = Mobility()
self.mvt = HsrMove()
self.listener = l()
self.utils = Utils()
self.listener.set_topic_and_typMEssage(
"/hsrb/arm_trajectory_controller/state",
JointTrajectoryControllerState)
#self.listener.set_topic_and_typMEssage("/whole_body_controller/state", JointTrajectoryControllerState)
def bottom_grasp_joints(self, arm_flex_joint_value):
"""
determinate joints values for arm_flex and wrist_flex
:param arm_flex_joint_value: value of arm_flex_joint
:type float
:return: arm_flex_joint, wrist_flex_joint
:type dict { arm_flex_joint: float, wrist_flex_joint: float}
"""
return {
"arm_flex_joint":
-arm_flex_joint_value,
"wrist_flex_joint":
round(abs(3.14 - arm_flex_joint_value + 1.57 - 3.14), 2)
}
def top_grasp_joints(self, arm_flex_joint_value):
"""
OPTIMAL AND BETTER
determinate joints values for arm_flex and wrist_flex
:param arm_flex_joint_value: value of arm_flex_joint
:type float
:return: arm_flex_joint, wrist_flex_joint
:type dict { arm_flex_joint: float, wrist_flex_joint: float}
"""
return {
"arm_flex_joint": -arm_flex_joint_value,
"wrist_flex_joint": round(-(3.14 - arm_flex_joint_value - 1.57), 2)
}
def arm_lift_value(self, hand_palm_pose_to_odom, object_pose_to_odom):
"""
This method gives the arm_lift_joint value
:param hand_palm_pose_to_odom: hand_palm_link to odom
:type Vector
:param object_pose_to_odom: object_pose to odom
:type Vector
:return: float
"""
h = object_pose_to_odom[2] - hand_palm_pose_to_odom[2]
return self.check_mobility.get_validated_value("arm_lift_joint", h)
def arm_flex_value(self, object_pose):
"""
this method determinate the arm_flex_joints value
:param object_pose_to_odom: object pose to odom
:type geometry_msgs.msg PoseStamped
:return: arm_flex_joint
:type float
"""
list_links = ["arm_flex_link", "wrist_flex_link"]
list_poses = [self.mvt.get_pose("map", x) for x in list_links]
print("fetch values of links")
print list_poses
# determine values of Positions to grasp
return self.mvt.get_angle_values(list_poses[0], list_poses[1],
object_pose)
def pre_pose_middle(self, object_pose):
"""
This method determines values of joints for middle pose
:param object_pose: object_pose
:return: dict of joints and values
"""
arm_flex = self.arm_flex_value(object_pose)
joints = self.top_grasp_joints(arm_flex) # rename top_joint
return joints
def pre_pose_top(self):
"""
This method determines values of joints for top pose
:param object_pose: object_pose
:return: dict of joints and values
"""
joints = self.top_grasp_joints(0.2) # rename top_joint
return joints
def pre_pose_bottom(self, arm_flex):
"""
This method determines values of joints for bottom pose
:param object_pose: object_pose
:return: dict of joints and values
"""
joints = self.top_grasp_joints(arm_flex) # renam top_joint
return joints
def pre_grasp_place_pose(self,
object_pose1,
object_pose_to_odom1,
up=0.0,
modus="FRONT"):
"""
This set the pre grasp pose of robot
:param object_pose: object_pose
:type Vector
:param object_pose_to_odom: object_pose to odom
:type Vector
:param up: up add more value to arm_lift
:type float
:param beside: decide if the grasp is from beside
:type bool
:param modus: decide typ of grasp
:type bool
:return: do move of pre grasp pose
"""
object_pose = self.mvt.parse_pose_to_array(object_pose1)
object_pose_to_odom = self.mvt.parse_pose_to_array(
object_pose_to_odom1)
if object_pose[2] <= 1 and object_pose[2] > 0.32:
joints = self.pre_pose_bottom(1.3) # 27.04.19
joints["wrist_roll_joint"] = 0.0
joints["arm_roll_joint"] = 0.0
# check if grasp is from top, then add
if modus == "TOP":
self.list_joints = self.utils.fusion_dict(
self.grasp_by_up(), joints)
self.list_joints[
"wrist_roll_joint"] = self.wrist_roll_value_for_top_pose(
object_pose1)
elif modus == "SIDE_RIGHT":
self.list_joints = self.utils.fusion_dict(
self.grasp_by_side_right(joints["arm_flex_joint"]), joints)
elif modus == "SIDE_LEFT":
self.list_joints = self.utils.fusion_dict(
self.grasp_by_side_left(joints["arm_flex_joint"]), joints)
else:
self.list_joints = joints
if modus == "TOP":
self.list_joints[
"arm_lift_joint"] = self.check_mobility.get_validated_value(
"arm_lift_joint",
float(up + object_pose_to_odom[2] - 0.278))
else:
self.list_joints[
"arm_lift_joint"] = self.check_mobility.get_validated_value(
"arm_lift_joint",
float(up + object_pose_to_odom[2] - 0.395581))
self.mvt.move_list_joints(self.list_joints) # 27.04.19
elif object_pose[2] <= 0.32:
# check if grasp is from top, then add
if modus == "TOP":
joints = self.pre_pose_bottom(1.8)
joints = self.utils.fusion_dict(self.grasp_by_up(), joints)
validated_joint = {}
for key in joints.keys():
validated_joint[
key] = self.check_mobility.get_validated_value(
key, joints[key])
# top pose wrist roll
validated_joint[
"wrist_roll_joint"] = self.wrist_roll_value_for_top_pose(
object_pose1)
validated_joint[
"arm_lift_joint"] = self.check_mobility.get_validated_value(
"arm_lift_joint",
float(up + object_pose_to_odom[2] - 0.11625))
self.mvt.move_list_joints(validated_joint)
else:
joints = self.pre_pose_bottom(2.4)
validated_joint = {}
validated_joint[
"arm_lift_joint"] = self.check_mobility.get_validated_value(
"arm_lift_joint",
up + object_pose_to_odom[2] - 0.07034)
for key in joints.keys():
validated_joint[
key] = self.check_mobility.get_validated_value(
key, joints[key])
self.mvt.move_list_joints(validated_joint)
if modus == "SIDE_RIGHT":
self.mvt.move_list_joints(
self.grasp_by_side_right(
validated_joint["arm_flex_joint"]))
elif modus == "SIDE_LEFT":
self.mvt.move_list_joints(
self.grasp_by_side_left(
validated_joint["arm_flex_joint"]))
else:
if modus == "TOP":
up = 0.0
joints = self.pre_pose_top()
self.mvt.move_list_joints(joints)
hand_palm_pose_to_odom = self.mvt.get_pose("map", "hand_palm_link")
self.mvt.move_list_joints({
"arm_lift_joint":
up + self.arm_lift_value(hand_palm_pose_to_odom,
object_pose_to_odom)
})
if modus == "SIDE_RIGHT":
self.mvt.move_list_joints(
self.grasp_by_side_right(joints["arm_flex_joint"]))
elif modus == "SIDE_LEFT":
self.mvt.move_list_joints(
self.grasp_by_side_left(joints["arm_flex_joint"]))
def end_pre_grasp(self):
"""
this method listens and performs special movements of the torso
:return:
"""
self.listener.listen_topic()
arm_lift = self.listener.get_value("arm_lift_joint")
self.mvt.move_list_joints({"arm_lift_joint": arm_lift + 0.03})
def end_grasp_place(self):
"""
This method execute the end of grasp pose
:return:
"""
self.mvt.end_grasp_pose_robot()
def pre_grasp_init(self):
"""
This method execute the pre grasp pose
:return:
"""
self.mvt.move_list_joints(self.mvt.states_pre_grasp)
def perceive_up(self):
"""
This method execute the perceive up pose, this help for better objects perception
:return:
"""
goal_js = {
"arm_lift_joint": 0.1,
"arm_flex_joint": -2.6,
"wrist_flex_joint": -1.70,
"arm_roll_joint": 1.57
}
self.mvt.move_list_joints(goal_js)
def perceive_side(self):
"""
This method execute the perceive side pose.
:return:
"""
goal_js = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": -1.57,
"arm_roll_joint": 0.0,
"arm_flex_joint": -0.3,
"arm_lift_joint": 0.0
}
self.mvt.move_list_joints(goal_js)
def grasp_by_side_right(self, arm_flex_value):
"""
calculates the rotation needed to catch from the right
:param arm_flex_value: float
:return: list of joints
"""
goal_js = {
"wrist_flex_joint": -1.57,
"wrist_roll_joint": arm_flex_value,
"arm_roll_joint": 1.57
}
return goal_js
def grasp_by_side_left(self, arm_flex_value):
"""
calculates the rotation needed to catch from the left
:param arm_flex_value: float
:return: list of joints
"""
goal_js = {
"wrist_flex_joint": -1.57,
"wrist_roll_joint": abs(arm_flex_value),
"arm_roll_joint": -1.57
}
return goal_js
def grasp_by_side(self, orientation):
p = PoseStamped()
hand_palm_pose = self.mvt.get_pose("map", "hand_palm_link")
p.header.frame_id = 'hand_palm_link'
p.pose.position.x = hand_palm_pose[0]
p.pose.position.y = hand_palm_pose[1]
p.pose.position.z = hand_palm_pose[2]
p.pose.orientation.x = orientation.x
p.pose.orientation.y = orientation.y
p.pose.orientation.z = orientation.z
p.pose.orientation.w = orientation.w
self.mvt.move_link_pose(p)
def grasp_by_up(self):
goal_js = {
"wrist_flex_joint": -1.57, #1.57
"arm_roll_joint": 0.0
}
return goal_js
def open_door_from_left(self):
"""
the method calculate the pose to open the left door
:return:
"""
goal_js = {
"wrist_flex_joint": -1.57,
"wrist_roll_joint": 1.57,
"arm_roll_joint": -1.57
}
return goal_js
def open_door_from_right(self):
"""
this method calculate the pose to open the right door
"""
goal_js = {
"wrist_flex_joint": -1.57,
"wrist_roll_joint": -1.57,
"arm_roll_joint": 1.57
}
return goal_js
def wrist_roll_value_for_top_pose(self, object_pose):
euler_rot = tf.transformations.euler_from_quaternion([
object_pose.orientation.x, object_pose.orientation.y,
object_pose.orientation.z, object_pose.orientation.w
])
print("Orientation object is:")
print object_pose.orientation
print euler_rot[2]
return euler_rot[2] + 1.57 #1.570796326795
def power_dB(power_in_W):
return 10.0 * np.log10(power_in_W)
def JFI_func(alist):
return (
(np.sum(alist))**2) / (1. * len(alist) * np.sum([x**2 for x in alist]))
const = Constants()
distribution = Speed_headway_random(scenario_flag="Urban_Nonpeak")
mobility = Mobility(
v0=120.0 * 1000. / 3600., # desired speed in m/s
T=1.6, # safe time headway in s
a=0.73, # maximum acceleration in m/s^2
b=1.67, # desired deceleration in m/s^2
delta=4.0, # acceleration exponent
s0=2.0, # minimum distance in m
l0=5.0)
lane_width = 3.0
communication_delay = 10.0 * 10**(
-3) # the delay in one-hop communication in second
movement_delay = 1.0
total_sim_time = 10.0
epoch_num = int(np.ceil(total_sim_time / communication_delay))
p_max = power_W(40.0) # p_max
initial_energy = p_max * communication_delay * epoch_num
sim_num = 100
class Resistivity(HelperFunctions):
cal_dts = {
'matterial': 'Si',
'temp': 300,
'mob_author': None,
'nieff_author': None,
'ionis_author': None,
'dopant': 'boron',
}
def __init__(self, **kwargs):
self._update_dts(**kwargs)
self._update_links()
def _update_links(self):
# setting downstream values, this should change from initalisation
# to just updating through the update function
self.Mob = Mob(matterial=self.cal_dts['matterial'],
author=self.cal_dts['mob_author'],
temp=self.cal_dts['temp'])
self.ni = ni(matterial=self.cal_dts['matterial'],
author=self.cal_dts['nieff_author'],
temp=self.cal_dts['temp'])
self.ion = Ion(matterial=self.cal_dts['matterial'],
author=self.cal_dts['ionis_author'],
temp=self.cal_dts['temp'])
def query_used_authors(self):
return self.Mob.model, self.ni.model, self.ion.model
def _conductivity(self, Na, Nd, nxc, **kwargs):
Nid, Nia = get_carriers(Na, Nd, 0,
temp=self.cal_dts['temp'],
ni_author=self.cal_dts['nieff_author']
)
if np.all(Nid > Nia):
Nid = self.ion.update_dopant_ionisation(
Nid, nxc, self.cal_dts['dopant'])
elif np.all(Nia > Nid):
Nia = self.ion.update_dopant_ionisation(
Nia, nxc, self.cal_dts['dopant'])
ne, nh = get_carriers(Nid, Nia, nxc,
temp=self.cal_dts['temp'],
ni_author=self.cal_dts['nieff_author']
)
mob_e = self.Mob.electron_mobility(nxc, Na, Nd,
temp=self.cal_dts['temp'])
mob_h = self.Mob.hole_mobility(nxc, Na, Nd,
temp=self.cal_dts['temp'])
# print mob_h, mob_e, Na
return const.e * (mob_e * ne + mob_h * nh)
def caculate(self, Na, Nd, nxc, **kwargs):
'''
caculates the resistivity
'''
self._update_dts(**kwargs)
self._update_links()
res = 1. / self._conductivity(Na, Nd, nxc, **kwargs)
return res
class HsrMove:
def __init__(self):
# initialize object
print("HSR_MOVE is actived")
# Set Mobility checker
self.check_mobility = Mobility()
self.interface = GiskardWrapper()
self.pose_stamped = PoseStamped()
self.states_pre_grasp = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": 0.0,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.place_states = {
"wrist_roll_joint": 0.0,
"wrist_flex_joint": -1.57,
"arm_roll_joint": 0.0,
"arm_flex_joint": 0.0,
"arm_lift_joint": 0.0
}
self.primary_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": -0.026,
"arm_roll_joint": -1.57,
"wrist_flex_joint": -1.57,
"wrist_roll_joint": 0.0
}
self.end_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.0,
"arm_roll_joint": 1.57
}
self.perceive_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.70,
"arm_roll_joint": 1.57
}
def init_robot(self):
""" initialize and set robot movement of the robot """
try:
print self.states
self.move_list_joints(self.states)
print("Start_grasp_pose is done")
except ValueError:
print "Parameter are invalid"
def end_grasp_pose_robot(self):
self.end_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.0,
"arm_roll_joint": 0.0
}
self.move_list_joints(self.end_states)
print("End_pose is done")
return True
def perceive_robot(self):
self.end_states = {
"arm_lift_joint": 0.0,
"arm_flex_joint": 0.0,
"wrist_flex_joint": -1.70,
"arm_roll_joint": 1.57
}
self.move_list_joints(self.end_states)
print("Perceive_pose is done")
return True
def do_move(self, move_parameter):
""" execute move of one joint or link """
if move_parameter[0] == "link":
param_frame_id = move_parameter[1]
param_x = move_parameter[2]
param_y = move_parameter[3]
param_z = move_parameter[4]
param_w = move_parameter[5]
self.move_link(param_frame_id, param_x, param_y, param_z, param_w)
print("Move is done")
elif move_parameter[0] == "joint":
param_frame_id = move_parameter[1]
param_value = move_parameter[2]
feedback_joint = self.move_joint(param_frame_id, param_value)
print("Move is done")
else:
print("command isn't sended")
print move_parameter[0]
def move_link(self, frame_id, x, y, z, w):
""" do move of link """
print("Frame_id " + str(frame_id) + ", x: " + str(x) + ", y: " +
str(y) + ", z: " + str(z) + ", w: " + str(w))
self.pose_stamped.header.frame_id = "map"
self.pose_stamped.pose.position.x = 6
self.pose_stamped.pose.position.y = 0.0
self.pose_stamped.pose.orientation.w = 1
self.interface.set_cart_goal('base_footprint', "base_link",
self.pose_stamped)
self.interface.plan_and_execute()
print("Move link is executed")
return True
def move_link_pose(self, pose):
""" do move of link """
self.pose_stamped.header.frame_id = "map"
self.pose_stamped.pose.position.x = pose.pose.position.x
self.pose_stamped.pose.position.y = pose.pose.position.y
self.pose_stamped.pose.position.z = pose.pose.position.z
self.pose_stamped.pose.orientation = pose.pose.orientation
self.interface.set_cart_goal('base_link', pose.header.frame_id,
self.pose_stamped)
if self.interface.plan_and_execute():
print("Move link is executed")
return True
return False
def get_coordinate(self, point):
"""convert string value to float """
if point != "none":
return float(point)
else:
return None
def move_joint(self, frame_id, value):
""" do move of joint """
print(str(frame_id), float(value))
if self.check_mobility.validate(str(frame_id), float(value)):
self.interface.set_joint_goal(
{str(frame_id): self.get_coordinate(value)})
self.interface.disable_self_collision()
self.interface.plan_and_execute()
print("Move is executed")
return True
else:
print("Move joint isn't executed")
return False
def move_list_joints(self, list_joints):
""" do move of joint """
print(list_joints)
if len(list_joints) > 0:
self.interface.set_joint_goal(list_joints)
self.interface.disable_self_collision()
self.interface.plan_and_execute()
print("List joints is executed")
return True
else:
print("List Joints is empty")
return False
def do_move_joints(self, joint_names, joint_values):
if (len(joint_names) == len(joint_names)) and len(joint_names) > 0:
for x in range(len(joint_names)):
check_value = self.check_mobility.get_validated_value(
joint_names[x], joint_values[x])
self.move_joint(joint_names[x], check_value)
print("End move joint")
else:
print("Joint list is empty")
def get_vector(self, point1, point2):
""" calcul and return a vector from two points """
return np.array(point2, float) - np.array(point1, float)
# same for angle by wrist_flex: wrist_flex_link_pose, hand_palm_link_pose, object_pose
def get_angle_values(self, middle_link_pose, grip_link_pose, object_pose):
"""
this method compute the angle between two links
:param middle_link_pose: 3D Vector
:param grip_link_pose: 3D Vector
:param object_pose: 3D Vector
:return: angle, float
"""
vect_middle_grip = self.get_vector(middle_link_pose, grip_link_pose)
vect_middle_obj = self.get_vector(middle_link_pose, object_pose)
return float(
np.arccos(
np.dot(vect_middle_grip, vect_middle_obj) /
(np.linalg.norm(vect_middle_grip) *
np.linalg.norm(vect_middle_obj))))
def get_arm_lift_up(self, hand_palm_link, object_pose):
"""
This method compute the height of the torso
:param arm_flex_link:
:param hand_palm_link:
:param object_pose:
:return: height, float
"""
h = (object_pose[2] - hand_palm_link[2])
return float(h)
def get_pose(self, source, frame_id):
"""
the method get exactly the pose of link frame_id to source(map or odom)
:param source: odom or map, typ string
:param frame_id: string
:return: [x, y, z] float array
"""
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
ps = PoseStamped()
transform = tf_buffer.lookup_transform(source, frame_id, rospy.Time(0),
rospy.Duration(5.0))
pose_transformed = tf2_geometry_msgs.do_transform_pose(ps, transform)
return [
pose_transformed.pose.position.x, pose_transformed.pose.position.y,
pose_transformed.pose.position.z
]
def get_msg_translation_and_rotation(self, source, frame_id):
"""
the method get exactly the pose of link frame_id to source(map or odom)
:param source: odom or map, typ string
:param frame_id: string
:return: translation, rotation(quaternion)
"""
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
ps = PoseStamped()
transform = tf_buffer.lookup_transform(source, frame_id, rospy.Time(0),
rospy.Duration(5.0))
return [transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z],\
[transform.transform.rotation.x, transform.transform.rotation.y, transform.transform.rotation.z, transform.transform.rotation.w]
def parse_pose_to_array(self, pose):
"""
this method get the exactly value of x, y and z from a pose msg
:param pose: geometry msg
:return: x, y, z float
"""
return [pose.position.x, pose.position.y, pose.position.z]
def get_distance(self, vec1, vec2):
"""
calculate the distance between two vectors
:param vec1:
:param vec2:
:return: distance float
"""
return float(np.linalg.norm(np.array(vec1) - np.array(vec2)))
def get_current_base_position(self):
"""
the method check the current pose of base_footprint to odom and get it
:return: x, y, rotation
"""
t, r = self.get_msg_translation_and_rotation("odom", "base_footprint")
return self.base_position(t, r)
def translate(self, position, translation_vector):
"""
the method does a translation on a position
:param position: current pose
:param translation_vector: translation vector
:return: vector[x, y], new position
"""
return position[0] + translation_vector[0], position[
1] + translation_vector[1]
def base_position(self, translation, rotation):
"""
the method take the translation and rotation from tf_ros and calculate the rotation in euler
:param translation: vector
:param rotation: quaternion (from tf_ros)
:return: vector[x, y, rotation], current base position
"""
euler = tf.transformations.euler_from_quaternion(rotation)
return translation[0], translation[1], euler[2]
def do_frame_rotatiom(self, source, frame_id, roll, pitch, yaw):
t, r = self.get_msg_translation_and_rotation(source, frame_id)
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame_id # u'map'
pose_stamped.pose.position.x = t[0]
pose_stamped.pose.position.y = t[1]
pose_stamped.pose.position.z = t[2]
pose_stamped.pose.orientation.x = quaternion[0]
pose_stamped.pose.orientation.y = quaternion[1]
pose_stamped.pose.orientation.z = quaternion[2]
pose_stamped.pose.orientation.w = quaternion[3]
return pose_stamped