Exemplo n.º 1
0
 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))
Exemplo n.º 2
0
 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))
Exemplo n.º 3
0
    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
Exemplo n.º 4
0
    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
Exemplo n.º 5
0
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()