def calculate_target_point(self, pose_comp):
self.__t_point.pose.position = pose_comp.primitive_poses[0].position
self.__t_point.pose.position.z += 0.3
quat = Quaternion()
quat.x = pose_comp.primitive_poses[0].orientation.x
quat.y = pose_comp.primitive_poses[0].orientation.y
quat.z = pose_comp.primitive_poses[0].orientation.z
quat.w = pose_comp.primitive_poses[0].orientation.w
print get_yaw(quat)
self.__t_point.pose.orientation = euler_to_quaternion(0, pi / 2, get_yaw(quat) + (pi / 2))
def calculate_target_point(self, pose_comp):
self.__t_point.pose.position = pose_comp.primitive_poses[0].position
self.__t_point.pose.position.z += 0.3
quat = Quaternion()
quat.x = pose_comp.primitive_poses[0].orientation.x
quat.y = pose_comp.primitive_poses[0].orientation.y
quat.z = pose_comp.primitive_poses[0].orientation.z
quat.w = pose_comp.primitive_poses[0].orientation.w
print get_yaw(quat)
self.__t_point.pose.orientation = euler_to_quaternion(
0, pi / 2,
get_yaw(quat) + (pi / 2))
def get_place_point(self, userdata, rad_fac=2):
target_zone_pose = geometry_msgs.msg.PoseStamped()
target_zone_pose.pose.position.x = random.uniform(-userdata.yaml.target_zones[0].max_distance / rad_fac,
userdata.yaml.target_zones[0].max_distance / rad_fac)
target_zone_pose.pose.position.y = random.uniform(-userdata.yaml.target_zones[0].max_distance / rad_fac,
userdata.yaml.target_zones[0].max_distance / rad_fac)
target_zone_pose.pose.position.z = 0
target_zone_pose.pose.orientation = geometry_msgs.msg.Quaternion(0.0, 0.0, 0.0, 1.0)
target_zone_pose.header.frame_id = "/target_zone"
self.__place_pose = utils.manipulation.transform_to(target_zone_pose)
self.__place_pose.pose.orientation = euler_to_quaternion(0, pi / 2, 0)
self.__place_pose.pose.position.z += 0.5
return self.__place_pose
def get_place_point(self, userdata, rad_fac=2):
target_zone_pose = geometry_msgs.msg.PoseStamped()
target_zone_pose.pose.position.x = random.uniform(
-userdata.yaml.target_zones[0].max_distance / rad_fac,
userdata.yaml.target_zones[0].max_distance / rad_fac)
target_zone_pose.pose.position.y = random.uniform(
-userdata.yaml.target_zones[0].max_distance / rad_fac,
userdata.yaml.target_zones[0].max_distance / rad_fac)
target_zone_pose.pose.position.z = 0
target_zone_pose.pose.orientation = geometry_msgs.msg.Quaternion(
0.0, 0.0, 0.0, 1.0)
target_zone_pose.header.frame_id = "/target_zone"
self.__place_pose = utils.manipulation.transform_to(target_zone_pose)
self.__place_pose.pose.orientation = euler_to_quaternion(0, pi / 2, 0)
self.__place_pose.pose.position.z += 0.5
return self.__place_pose
from math import pi
from geometry_msgs.msg import Quaternion
import rospy
from suturo_planning_manipulation.force_test import plan_to
from suturo_planning_manipulation.manipulation import Manipulation, geometry_msgs
from suturo_planning_manipulation.mathemagie import euler_to_quaternion
__author__ = 'benny'
if __name__ == '__main__':
rospy.init_node('force_test', anonymous=True)
m = Manipulation()
t_point = geometry_msgs.msg.Pose()
t_point.position.x = -0.4
t_point.position.y = 0
t_point.position.z = 0.3
t_point.orientation = euler_to_quaternion(0, pi, 0)
m.direct_move(plan_to(t_point, m))
m.open_gripper()
