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()