def insert6DofGlobalMarker(self, T_W_M):
        int_position_marker = InteractiveMarker()
        int_position_marker.header.frame_id = 'world'
        int_position_marker.name = 'obj_pose_marker'
        int_position_marker.scale = 0.1
        int_position_marker.pose = pm.toMsg(T_W_M)
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'x'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'y'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.ROTATE_AXIS,'z'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'x'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'y'));
        int_position_marker.controls.append(self.createInteractiveMarkerControl6DOF(InteractiveMarkerControl.MOVE_AXIS,'z'));
        self.mk_server.insert(int_position_marker, self.processFeedback)

        int_button_marker = InteractiveMarker()
        int_button_marker.header.frame_id = 'world'
        int_button_marker.name = 'obj_button_marker'
        int_button_marker.scale = 0.2
        int_button_marker.pose = pm.toMsg(PyKDL.Frame())
        box = self.createButtonMarkerControl(Point(0.05,0.05,0.05), Point(0.0, 0.0, 0.15) )
        box.interaction_mode = InteractiveMarkerControl.BUTTON
        box.name = 'button'
        int_button_marker.controls.append( box )
        self.mk_server.insert(int_button_marker, self.processFeedback)

        self.mk_server.applyChanges()
Esempio n. 2
0
    def step(self):
        if self.T_ is None:
            self.init_tf()
            return

        if self.recv_:
            self.recv_ = False
            pose = self.odom_.pose.pose
            T0 = pm.toMatrix(pm.fromMsg(pose))  # android_odom --> android

            # send odom -> base_link transform
            T = tf.transformations.concatenate_matrices(self.T_, T0, self.Ti_)
            frame = pm.fromMatrix(T)
            if self.pub_tf_:
                txn, qxn = pm.toTf(frame)
                self.tfb_.sendTransform(txn, qxn, self.odom_.header.stamp,
                                        'odom', 'base_link')

            # send as msg
            # TODO : does not deal with twist/covariance information
            msg = pm.toMsg(frame)
            self.cvt_msg_.pose.pose = pm.toMsg(frame)
            self.cvt_msg_.header.frame_id = 'map'  # experimental
            self.cvt_msg_.header.stamp = self.odom_.header.stamp
            self.pub_.publish(self.cvt_msg_)
Esempio n. 3
0
    def pose_callback(self, pose):
        # This code is based on:
        # https://github.com/ros-planning/navigation/blob/jade-devel\
        #              /amcl/src/amcl_node.cpp

        try:
            self.tf_listener.waitForTransform(
                'map',  # from here
                'odom',  # to here
                pose.header.stamp,
                rospy.Duration(1.0))
            frame = posemath.fromMsg(pose.pose).Inverse()
            pose.pose = posemath.toMsg(frame)
            pose.header.frame_id = 'base_link'

            odom_pose = self.tf_listener.transformPose('odom', pose)
            frame = posemath.fromMsg(odom_pose.pose).Inverse()
            odom_pose.pose = posemath.toMsg(frame)

            self.transform_position[0] = odom_pose.pose.position.x
            self.transform_position[1] = odom_pose.pose.position.y
            self.transform_quaternion[0] = odom_pose.pose.orientation.x
            self.transform_quaternion[1] = odom_pose.pose.orientation.y
            self.transform_quaternion[2] = odom_pose.pose.orientation.z
            self.transform_quaternion[3] = odom_pose.pose.orientation.w

        except tf.Exception as e:
            print e
            print "(May not be a big deal.)"
    def compute_frames(self,req):
            #read nominal poses, and set as initial positions
    
            self.crc.set_intial_frames(posemath.fromMsg( self.w_P_c),
                                        posemath.fromMsg(self.ee_P_m))

            
            #do several iteration of estimation

            n_comp=6
            residue_max=[]
            residue_mod=[]
            for i in range(n_comp):
                print '\ncurrent position'
                print self.crc.w_T_c.p
                residue= self.crc.compute_frames();
                r2=residue.transpose()*residue
                residue_mod.append( num.sqrt (r2[0,0]))
                residue_max.append(num.max(num.abs(residue)))
            print '\nresidue_mod'
            print residue_mod
            print '\nresidue_max'
            print residue_max
            #put result back in parameter
            print '\nee_T_m'
            print self.crc.ee_T_m
            print '\nw_T_c'
            print self.crc.w_T_c
            self.ee_P_m = posemath.toMsg(self.crc.ee_T_m)
            self.w_P_c=posemath.toMsg(self.crc.w_T_c)
            
            return EmptyResponse();
Esempio n. 5
0
def irp6p_multi_trajectory2():
	irpos = IRPOS("IRpOS", "Irp6p", 6)

	irpos.move_to_motor_position([0.4, -1.5418065817051163, 0.0, 1.57, 1.57, -2.0], 10.0)
	irpos.move_to_motor_position([10.0, 10.0, 0.0, 10.57, 10.57, -20.0], 2.0)

	irpos.move_to_joint_position([0.4, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], 3.0)
	irpos.move_to_joint_position([0.0, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], 3.0)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.18029263241))
	rot2 = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.3, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	irpos.move_to_cartesian_pose(3.0, Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot2))

	toolParams = Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	irpos.move_to_cartesian_pose(3.0,Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)))
	irpos.move_to_cartesian_pose(3.0,pm.toMsg(rot))
	irpos.move_to_cartesian_pose(3.0,Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.63691, 0.096783, 0.75634, -0.11369)))

	toolParams = Pose(Point(0.0, 0.0, 0.25), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	print "Irp6p 'multi_trajectory2' test compleated"
    def test_current_pose(self):
        """ Test the current pose method

            1. create and publish tf
            2. get current pose with base and ref
            3. move roboter to ref
            4. get current pose with ref and base
            5. analyse positions
        """
        rospy.loginfo("test_current_pose")

        self.robot.move(Ptp(goal=[0, 0, 0, 0, 0, 0]))

        # get and transform test data
        ref_frame = self.test_data.get_pose("Blend_1_Mid", PLANNING_GROUP_NAME)
        ref_frame_tf = pm.toTf(pm.fromMsg(ref_frame))

        rospy.sleep(rospy.Duration.from_sec(0.5))

        # init
        tcp_ref = [[0, 0, 0], [0, 0, 0, 1]]
        tcp_base = [[0, 0, 0], [0, 0, 0, 1]]
        time_tf = rospy.Time.now()
        base_frame = self.robot._robot_commander.get_planning_frame()

        try:
            self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1],
                                  time_tf, "ref_frame", base_frame)

            self.tf_listener.waitForTransform(base_frame, "ref_frame", time_tf, rospy.Duration(2, 0))
            rospy.sleep(rospy.Duration.from_sec(0.1))
            tcp_ref = self.tf_listener.lookupTransform("ref_frame", "prbt_tcp", time_tf)
            tcp_base = self.tf_listener.lookupTransform(base_frame, "prbt_tcp", time_tf)
        except Exception as e:
            print e
            rospy.logerr("Failed to setup transforms for test!")

        tcp_ref_msg = pm.toMsg(pm.fromTf(tcp_ref))
        tcp_base_msg = pm.toMsg(pm.fromTf(tcp_base))

        # read current pose, move robot and do it again
        start_bf = self.robot.get_current_pose()
        start_rf = self.robot.get_current_pose(base="ref_frame")

        self.robot.move(Ptp(goal=ref_frame))

        # resending tf (otherwise transform pose could read old transform to keep time close together.
        time_tf = rospy.Time.now()
        self.tf.sendTransform(ref_frame_tf[0], ref_frame_tf[1], time_tf, "ref_frame", base_frame)
        self.tf_listener.waitForTransform(base_frame, "ref_frame", time_tf, rospy.Duration(1, 0))

        ref_frame_bf = self.robot.get_current_pose()
        ref_frame_rf = self.robot.get_current_pose(base="ref_frame")

        self._analyze_pose(tcp_base_msg, start_bf)
        self._analyze_pose(tcp_ref_msg, start_rf)
        self._analyze_pose(ref_frame, ref_frame_bf)
        self._analyze_pose(Pose(orientation=Quaternion(0, 0, 0, 1)), ref_frame_rf)
    def TFThread(self):
        while not rospy.is_shutdown():
            small_frame_list = []
            big_frame_list = []
            self_frame = self.robot_id + "/map"
            for tf_frame, tf in self.tf_between_robots_dict.items():
                if tf_frame[:len(self_frame)] == self_frame:
                    big_frame_list.append(tf.child_frame_id)
                elif tf_frame[-len(self_frame):] == self_frame:
                    small_frame_list.append(tf.header.frame_id)

            small_frame_list.sort()
            for i in range(1, len(small_frame_list)):
                tf1 = self.tf_between_robots_dict[small_frame_list[i - 1] +
                                                  ' to ' + self_frame]
                tf2 = self.tf_between_robots_dict[small_frame_list[i] +
                                                  ' to ' + self_frame]
                pose1 = posemath.fromMsg(trans2pose(tf1))
                pose2 = posemath.fromMsg(trans2pose(tf2))
                tf = TransformStamped()
                tf.header.stamp = rospy.Time.now()
                tf.header.frame_id = tf1.header.frame_id
                tf.child_frame_id = tf2.header.frame_id
                tf.transform = pose2trans(
                    posemath.toMsg(pose1 * pose2.Inverse()))
                print("publish tf: " + tf.header.frame_id + " to " +
                      tf.child_frame_id)
                self.br.sendTransformMessage(tf)
            if len(small_frame_list) > 0:
                print("publish tf: " + small_frame_list[-1] + ' to ' +
                      self_frame)
                self.br.sendTransformMessage(
                    self.tf_between_robots_dict[small_frame_list[-1] + ' to ' +
                                                self_frame])

            big_frame_list.sort()
            if len(big_frame_list) > 0:
                print("publish tf: " + self_frame + ' to ' + big_frame_list[0])
                self.br.sendTransformMessage(
                    self.tf_between_robots_dict[self_frame + ' to ' +
                                                big_frame_list[0]])
            for i in range(1, len(big_frame_list)):
                tf1 = self.tf_between_robots_dict[self_frame + ' to ' +
                                                  big_frame_list[i - 1]]
                tf2 = self.tf_between_robots_dict[self_frame + ' to ' +
                                                  big_frame_list[i]]
                pose1 = posemath.fromMsg(trans2pose(tf1))
                pose2 = posemath.fromMsg(trans2pose(tf2))
                tf = TransformStamped()
                tf.header.stamp = rospy.Time.now()
                tf.header.frame_id = tf1.child_frame_id
                tf.child_frame_id = tf2.child_frame_id
                tf.transform = pose2trans(
                    posemath.toMsg(pose1.Inverse() * pose2))
                print("publish tf: " + tf.header.frame_id + " to " +
                      tf.child_frame_id)
                self.br.sendTransformMessage(tf)
            time.sleep(1)
Esempio n. 8
0
def _getCollisionObject(name, urdf, pose, operation):
    '''
    This function takes an urdf and a TF pose and updates the collision
    geometry associated with the current planning scene.
    '''
    co = CollisionObject()
    co.id = name
    co.operation = operation

    if len(urdf.links) > 1:
        rospy.logwarn(
            'collison object parser does not currently support kinematic chains'
        )

    for l in urdf.links:
        # Compute the link pose based on the origin
        if l.origin is None:
            link_pose = pose
        else:
            link_pose = pose * kdl.Frame(kdl.Rotation.RPY(*l.origin.rpy),
                                         kdl.Vector(*l.origin.xyz))
        for c in l.collisions:
            # Only update the geometry if we actually need to add the
            # object to the collision scene.
            # check type of each collision tag.
            if isinstance(c.geometry, urdf_parser_py.urdf.Box):
                primitive = True
                if co.operation == CollisionObject.ADD:
                    size = c.geometry.size
                    element = SolidPrimitive()
                    element.type = SolidPrimitive.BOX
                    element.dimensions = list(c.geometry.size)
                    co.primitives.append(element)
            elif isinstance(c.geometry, urdf_parser_py.urdf.Mesh):
                primitive = False
                if co.operation == CollisionObject.ADD:
                    scale = (1, 1, 1)
                    if c.geometry.scale is not None:
                        scale = c.scale
                    element = _loadMesh(c.geometry, scale)
                    co.meshes.append(element)
            else:
                raise NotImplementedError(
                    "we do not currently support geometry of type %s" %
                    (str(type(c.geometry))))

            pose = kdl.Frame(kdl.Rotation.RPY(*c.origin.rpy),
                             kdl.Vector(*c.origin.xyz))
            pose = link_pose * pose
            if primitive:
                co.primitive_poses.append(pm.toMsg(pose))
            else:
                # was a mesh
                co.mesh_poses.append(pm.toMsg(pose))

    return co
    def set_pose_frame(self, frame1, frame2):
        """

        :param frame: PyKDL.Frame
        """
        msg1 = posemath.toMsg(frame1)
        msg2 = posemath.toMsg(frame2)
        self.__set_position_goal_cartesian_publish_and_wait(msg1)
        self.__set_position_goal_cartesian_publish_and_wait(msg2)
        return True
Esempio n. 10
0
def oplus(cur_estimate, step):
    result = deepcopy(cur_estimate)

    # loop over camera's
    for camera, res, camera_index in zip(cur_estimate.cameras, result.cameras, [r*pose_width for r in range(len(cur_estimate.cameras))]):
        res.pose = posemath.toMsg(pose_oplus(posemath.fromMsg(camera.pose), step[camera_index:camera_index+pose_width]))
    # loop over targets
    for i, target in enumerate(cur_estimate.targets): 
        target_index = (len(cur_estimate.cameras) + i) * pose_width
        result.targets[i] = posemath.toMsg(pose_oplus(posemath.fromMsg(target), step[target_index:target_index+pose_width]))

    return result
Esempio n. 11
0
    def compute_net_transform(self, base_pose):

        base_to_odom = self.tfl.lookupTransform("/base_footprint", "/odom_combined", rospy.Time())

        bto = pm.fromTf(base_to_odom)
        

        net_t = base_pose * bto

        print "Setting "
        print pm.toMsg(net_t)
        self.broad.transform = pm.toTf(net_t)
Esempio n. 12
0
 def _dataToPose(self,data):
   '''
   Get visualization information as a vector of poses for whatever object we
   are currently manipulating.
   '''
   msg = PoseArray()
   for traj in data:
     for world in traj:
       if world['orange'] is not None:
         msg.poses.append(pm.toMsg(world['orange']))
       msg.poses.append(pm.toMsg(world['box']))
       msg.poses.append(pm.toMsg(world['trash']))
       msg.poses.append(pm.toMsg(world['squeeze_area']))
   return msg
Esempio n. 13
0
def omni_callback(joint_state):
    global update_pub, last_button_state
    sendTf(marker_tf, '/marker', fixed_frame)
    sendTf(zero_tf, '/base', fixed_frame)
    sendTf(marker_ref, '/marker_ref', fixed_frame)
    sendTf(stylus_ref, '/stylus_ref', fixed_frame)
        
    try:
        update = InteractionCursorUpdate()
        update.pose.header = std_msgs.msg.Header()
        update.pose.header.stamp = rospy.Time.now()
        update.pose.header.frame_id = 'marker_ref'
        if button_clicked and last_button_state == update.GRAB:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked and last_button_state == update.KEEP_ALIVE:
            update.button_state = update.KEEP_ALIVE
        elif button_clicked:
            update.button_state = update.GRAB
        elif last_button_state == update.KEEP_ALIVE:
            update.button_state = update.RELEASE
        else:
            update.button_state = update.NONE
            updateRefs()
        update.key_event = 0

        control_tf = ((0, 0, 0), tf.transformations.quaternion_from_euler(0, 0, control_offset))
        if button_clicked:
            # Get pose corresponding to transform between stylus reference and current position.
            stylus_tf = listener.lookupTransform('/stylus_ref', '/stylus', rospy.Time(0))
            stylus_matrix = pm.toMatrix(pm.fromTf(stylus_tf))
            control_matrix = pm.toMatrix(pm.fromTf(control_tf))
            p = pm.toMsg(pm.fromMatrix(numpy.dot(control_matrix, stylus_matrix)))
        else:
            p = pm.toMsg(pm.fromTf(control_tf))
        
        # Simply scale this up a bit to increase the workspace.
        workspace = 4
        p.position.x = p.position.x * workspace
        p.position.y = p.position.y * workspace
        p.position.z = p.position.z * workspace

        update.pose.pose = p

        last_button_state = update.button_state

        # Publish feedback.
        update_pub.publish(update)
    except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
        rospy.logerr("Couldn't look up transform. These things happen...")
Esempio n. 14
0
    def moveEffector(self, prefix, T_B_Td, t, max_wrench, start_time=0.01, stamp=None, path_tol=None):
        behaviour = self.getControllerBehaviour()
        if behaviour != self.BEHAVOIUR_CART_IMP and behaviour != self.BEHAVOIUR_CART_IMP_FT:
            print "moveEffector " + prefix + ": wrong behaviour " + self.getBehaviourName(behaviour)
            return False

        self.joint_traj_active = False
        wrist_pose = pm.toMsg(T_B_Td)
#        self.br.sendTransform([wrist_pose.position.x, wrist_pose.position.y, wrist_pose.position.z], [wrist_pose.orientation.x, wrist_pose.orientation.y, wrist_pose.orientation.z, wrist_pose.orientation.w], rospy.Time.now(), "dest", "torso_base")

        action_trajectory_goal = CartesianTrajectoryGoal()
        if stamp != None:
            action_trajectory_goal.trajectory.header.stamp = stamp
        else:
            action_trajectory_goal.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(start_time)
        action_trajectory_goal.trajectory.points.append(CartesianTrajectoryPoint(
        rospy.Duration(t),
        wrist_pose,
        Twist()))
        action_trajectory_goal.wrench_constraint = self.wrenchKDLtoROS(max_wrench)
        if path_tol != None:
            action_trajectory_goal.path_tolerance.position = geometry_msgs.msg.Vector3( path_tol.vel.x(), path_tol.vel.y(), path_tol.vel.z() )
            action_trajectory_goal.path_tolerance.rotation = geometry_msgs.msg.Vector3( path_tol.rot.x(), path_tol.rot.y(), path_tol.rot.z() )
        self.current_max_wrench = self.wrenchKDLtoROS(max_wrench)
        self.action_cart_traj_client[prefix].send_goal(action_trajectory_goal, feedback_cb = self.action_right_cart_traj_feedback_cb)
        return True
Esempio n. 15
0
    def getPlanWaypoints(self,waypoints_in_kdl_frame,q,obj=None):
      cartesian_path_req = GetCartesianPathRequest()
      cartesian_path_req.header.frame_id = self.base_link
      cartesian_path_req.start_state = RobotState()
      cartesian_path_req.start_state.joint_state.name = self.joint_names
      if type(q) is list:
        cartesian_path_req.start_state.joint_state.position = q
      else:
        cartesian_path_req.start_state.joint_state.position = q.tolist()
      cartesian_path_req.group_name = self.group
      cartesian_path_req.link_name = self.joint_names[-1]
      cartesian_path_req.avoid_collisions = False
      cartesian_path_req.max_step = 50
      cartesian_path_req.jump_threshold = 0
      # cartesian_path_req.path_constraints = Constraints()

      if obj is not None:
        self.updateAllowedCollisions(obj,True)
      
      cartesian_path_req.waypoints = list()

      for T in waypoints_in_kdl_frame:
        cartesian_path_req.waypoints.append(pm.toMsg(T))

      res = self.cartesian_path_plan.call(cartesian_path_req)

      if obj is not None:
        self.updateAllowedCollisions(obj,False)

      return (res.error_code.val, res)
Esempio n. 16
0
    def execute(self, wait=True, plan_only=False):
        """ Plan and execute cartesian path thru specified waypoints """
        waypoints = [pm.toMsg(f) for f in self._frames]
        (plan, frac) = self._arm.compute_cartesian_path(
            waypoints, eef_step=0.005, jump_threshold=5.0)
        if frac < 1.0:
            return False
        if plan_only:
            return True

        t = rospy.Duration()
        points = []
        for p in plan.joint_trajectory.points:
            if p.time_from_start != t:
                points.append(p)
                t = p.time_from_start
        plan.joint_trajectory.points = points

        if not self._arm.execute(plan, wait):
            current = self._arm.get_current_joint_values()
            desired = plan.joint_trajectory.points[-1].positions
            if not np.allclose(current, desired, atol=0.02):
                return False

        return True
Esempio n. 17
0
def callback(m):
    global pub
    global stop
    global data
    global tfListener
    global numberOfFootstepsInList
    if ready:
        if not stop:
            print('Preparing the footstep message')
            # Wait for transform
            tfListener.waitForTransform("/pelvis", "/leftCOP_Frame",
                                        rospy.Time(), rospy.Duration(4.0))
            tfListener.waitForTransform("/pelvis", "/rightCOP_Frame",
                                        rospy.Time(), rospy.Duration(4.0))

            # Get current robot pose
            pos1, rot1 = tfListener.lookupTransform("/pelvis",
                                                    "/leftCOP_Frame",
                                                    rospy.Time())
            pos2, rot2 = tfListener.lookupTransform("/pelvis",
                                                    "/rightCOP_Frame",
                                                    rospy.Time())

            pos = (np.array(pos1) + np.array(pos2)) * 0.5
            rot = pm.transformations.quaternion_slerp(rot1, rot2, 0.5)
            midFeet = pm.Frame(
                pm.Rotation.Quaternion(rot[0], rot[1], rot[2], rot[3]),
                pm.Vector(pos[0], pos[1], pos[2]))
            pose = pm.fromMsg(m.pose.pose) * midFeet

            # Get footstep trajectopry from YAML
            message = message_converter.convert_dictionary_to_ros_message(
                'controller_msgs/FootstepDataListMessage', data)
            # Update the footstep frames
            numberOfFootstepsInList = len(message.footstep_data_list)
            for step in message.footstep_data_list:
                pstep = pose * pm.Frame(
                    pm.Rotation.Quaternion(
                        step.orientation.x, step.orientation.y,
                        step.orientation.z, step.orientation.w),
                    pm.Vector(step.location.x, step.location.y,
                              step.location.z))
                msg = pm.toMsg(pstep)
                step.location = msg.position
                # Normalize orientation
                quat_original = np.array([
                    msg.orientation.x, msg.orientation.y, msg.orientation.z,
                    msg.orientation.w
                ])
                quat_normalized = quat_original / np.linalg.norm(quat_original)
                msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = quat_normalized[
                    0], quat_normalized[1], quat_normalized[
                        2], quat_normalized[3]
                # Update step orientation
                step.orientation = msg.orientation
            print('Relocated footsteps to a local frame')
            print('Publishing...')
            pub.publish(message)
            stop = True
            print('Waiting for execution...')
Esempio n. 18
0
def irp6p_multi_trajectory():
	irpos = IRPOS("IRpOS", "Irp6p", 6)

	motor_trajectory = [JointTrajectoryPoint([0.4, -1.5418065817051163, 0.0, 1.57, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(10.0)), JointTrajectoryPoint([10.0, 10.0, 0.0, 10.57, 10.57, -20.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(12.0))]
        irpos.move_along_motor_trajectory(motor_trajectory)

	joint_trajectory = [JointTrajectoryPoint([0.4, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(3.0)),JointTrajectoryPoint([0.0, -1.5418065817051163, 0.0, 1.5, 1.57, -2.0], [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], [], [], rospy.Duration(6.0))]
	irpos.move_along_joint_trajectory(joint_trajectory)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.18029263241))
	rot2 = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.3, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	cartesianTrajectory = [CartesianTrajectoryPoint(rospy.Duration(3.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)), Twist()), CartesianTrajectoryPoint(rospy.Duration(6.0), pm.toMsg(rot), Twist()),CartesianTrajectoryPoint(rospy.Duration(9.0), pm.toMsg(rot2), Twist())]
	irpos.move_along_cartesian_trajectory(cartesianTrajectory)

	toolParams = Pose(Point(0.0, 0.0, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	rot = PyKDL.Frame(PyKDL.Rotation.EulerZYZ(0.0, 1.4, 3.14), PyKDL.Vector(0.705438961242, -0.1208864692291, 1.181029263241))
	cartesianTrajectory = [CartesianTrajectoryPoint(rospy.Duration(3.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.675351045979, 0.0892025112399, 0.698321120995, 0.219753244928)), Twist()),
CartesianTrajectoryPoint(rospy.Duration(6.0), pm.toMsg(rot), Twist()),CartesianTrajectoryPoint(rospy.Duration(9.0), Pose(Point(0.705438961242, -0.1208864692291, 1.181029263241), Quaternion(0.63691, 0.096783, 0.75634, -0.11369)), Twist())]
	irpos.move_along_cartesian_trajectory(cartesianTrajectory)

	toolParams = Pose(Point(0.0, 0.0, 0.25), Quaternion(0.0, 0.0, 0.0, 1.0))
	irpos.set_tool_geometry_params(toolParams)

	print "Irp6p 'multi_trajectory' test compleated"
Esempio n. 19
0
    def gotimage(self, imgmsg):
        if imgmsg.encoding == "bayer_bggr8":
            imgmsg.encoding = "8UC1"
        img = CvBridge().imgmsg_to_cv(imgmsg)
        # cv.ShowImage("DataMatrix", img)
        # cv.WaitKey(6)

        self.track(img)

        # monocular case
        print self.cams
        if 'l' in self.cams:
            for (code, corners) in self.tracking.items():
                model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))

                rot = cv.CreateMat(3, 1, cv.CV_32FC1)
                trans = cv.CreateMat(3, 1, cv.CV_32FC1)
                cv.FindExtrinsicCameraParams2(model,
                                              cv.fromarray(numpy.array(corners, numpy.float32)),
                                              self.cams['l'].intrinsicMatrix(),
                                              self.cams['l'].distortionCoeffs(),
                                              rot,
                                              trans)

                ts = geometry_msgs.msg.TransformStamped()
                ts.header.frame_id = imgmsg.header.frame_id
                ts.header.stamp = imgmsg.header.stamp
                ts.child_frame_id = code
                posemsg = pm.toMsg(pm.fromCameraParams(cv, rot, trans))
                ts.transform.translation = posemsg.position
                ts.transform.rotation = posemsg.orientation

                tfm = tf.msg.tfMessage([ts])
                self.pub_tf.publish(tfm)
Esempio n. 20
0
def Transarray2G2O(g2ofname, trans_array):
    index = 1
    prev_pose = Pose()
    prev_pose.orientation.w = 1
    rel_pose = Pose()

    with open(g2ofname, "w") as g2ofile:
        for posetrans in trans_array.transformArray:
            pose = trans2pose(posetrans.transform)
            if index == 1:
                pass
            else:
                rel_pose = posemath.toMsg(
                    (posemath.fromMsg(prev_pose).Inverse() *
                     posemath.fromMsg(pose)))
                # tmp = posemath.toMsg( posemath.fromMsg(prev_pose)*posemath.fromMsg(rel_pose) )
                prev_pose = pose
                print >> g2ofile, "EDGE_SE3:QUAT {id1} {id2} {tx} {ty} {tz} {rx} {ry} {rz} {rw}  {COVAR_STR}".format(
                    id1=posetrans.child_frame_id,
                    id2=posetrans.header.frame_id,
                    tx=rel_pose.position.x,
                    ty=rel_pose.position.y,
                    tz=rel_pose.position.z,
                    rx=rel_pose.orientation.x,
                    ry=rel_pose.orientation.y,
                    rz=rel_pose.orientation.z,
                    rw=rel_pose.orientation.w,
                    COVAR_STR=COVAR_STR)

            # print >> g2ofile, "VERTEX_SE3:QUAT {pointID} {tx} {ty} {tz} {rx} {ry} {rz} {rw}".format(pointID=posetrans.header.frame_id,tx=pose.position.x,ty=pose.position.y,tz=pose.position.z,
            #     rx = pose.orientation.x, ry = pose.orientation.y, rz = pose.orientation.z, rw = pose.orientation.w)

            index += 1
Esempio n. 21
0
	def move_rel_to_cartesian_pose_with_contact(self, time_from_start, rel_pose, wrench_constraint):
		print self.BCOLOR+"[IRPOS] Move relative to cartesian trajectory with contact"+self.ENDC
				
		self.conmanSwitch([self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], [], True)
		time.sleep(0.05)

		actual_pose = self.get_cartesian_pose()
		
		# Transform poses to frames.
		actualFrame = pm.fromMsg(actual_pose)
		
		relativeFrame = pm.fromMsg(rel_pose)
		
		desiredFrame = actualFrame * relativeFrame
		
		pose = pm.toMsg(desiredFrame)

		cartesianGoal = CartesianTrajectoryGoal()
		cartesianGoal.wrench_constraint = wrench_constraint
		cartesianGoal.trajectory.points.append(CartesianTrajectoryPoint(rospy.Duration(time_from_start), pose, Twist()))
		cartesianGoal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)

		self.pose_client.send_goal(cartesianGoal)
		self.pose_client.wait_for_result()

		result = self.pose_client.get_result()
		code = self.cartesian_error_code_to_string(result.error_code)
		print self.BCOLOR+"[IRPOS] Result: "+str(code)+self.ENDC

		self.conmanSwitch([], [self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], True)
Esempio n. 22
0
    def set_pose_frame(self, frame):
        """

        :param frame: PyKDL.Frame
        """
        msg = posemath.toMsg(frame)
        return self.__set_position_goal_cartesian_publish_and_wait(msg)
    def get_new_waypoint(self,obj):
        try:
            # TODO: make the snap configurable
            #(trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
            (eg_trans,eg_rot) = self.listener.lookupTransform(self.gripper_center,self.endpoint,rospy.Time(0))
            (og_trans,og_rot) = self.listener.lookupTransform(obj,self.gripper_center,rospy.Time(0))

            rospy.logwarn("gripper obj:" + str((og_trans, og_rot)))
            rospy.logwarn("endpoint gripper:" + str((eg_trans, eg_rot)))

            xyz, rpy = [], []
            for dim in og_trans:
                # TODO: test to see if we should re-enable this
                if abs(dim) < 0.0001:
                    xyz.append(0.)
                else:
                    xyz.append(dim)
            Rog = kdl.Rotation.Quaternion(*og_rot)
            for dim in Rog.GetRPY():
                rpy.append(np.round(dim / np.pi * 8.) * np.pi/8.)
            Rog_corrected = kdl.Rotation.RPY(*rpy)
            Vog_corrected = kdl.Vector(*xyz)
            Tog_corrected = kdl.Frame(Rog_corrected, Vog_corrected)

            rospy.logwarn(str(Tog_corrected) + ", " + str(Rog_corrected.GetRPY()))

            Teg = pm.fromTf((eg_trans, eg_rot))
            return pm.toMsg(Tog_corrected * Teg)
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
        return None
Esempio n. 24
0
    def getPlanWaypoints(self, waypoints_in_kdl_frame, q, obj=None):
        cartesian_path_req = GetCartesianPathRequest()
        cartesian_path_req.header.frame_id = self.base_link
        cartesian_path_req.start_state = RobotState()
        cartesian_path_req.start_state.joint_state.name = self.joint_names
        if type(q) is list:
            cartesian_path_req.start_state.joint_state.position = q
        else:
            cartesian_path_req.start_state.joint_state.position = q.tolist()
        cartesian_path_req.group_name = self.group
        cartesian_path_req.link_name = self.joint_names[-1]
        cartesian_path_req.avoid_collisions = False
        cartesian_path_req.max_step = 50
        cartesian_path_req.jump_threshold = 0
        # cartesian_path_req.path_constraints = Constraints()

        if obj is not None:
            self.updateAllowedCollisions(obj, True)

        cartesian_path_req.waypoints = list()

        for T in waypoints_in_kdl_frame:
            cartesian_path_req.waypoints.append(pm.toMsg(T))

        res = self.cartesian_path_plan.call(cartesian_path_req)

        if obj is not None:
            self.updateAllowedCollisions(obj, False)

        return (res.error_code.val, res)
Esempio n. 25
0
 def transformPose(self, pose, targetFrame):
     with self._lock:
         tTargetFramePoseFrame = None
         for trial in range(10):
             self.checkPreemption()
             try:
                 tTargetFramePoseFrame = self._tfListener.lookupTransform(
                     targetFrame, pose.header.frame_id, rospy.Time(0))
                 break
             except (tf.LookupException, tf.ConnectivityException,
                     tf.ExtrapolationException):
                 rospy.sleep(0.5)
         if tTargetFramePoseFrame is None:
             raise ValueError('Could not transform pose from ' +
                              pose.header.frame_id + ' to ' + targetFrame)
         # Convert poses to KDL
         fTargetFramePoseFrame = posemath.fromTf(tTargetFramePoseFrame)
         fPoseFrameChild = posemath.fromMsg(pose.pose)
         # Compute transformation
         fTargetFrameChild = fTargetFramePoseFrame * fPoseFrameChild
         # Convert back to ROS message
         poseStampedTargetFrameChild = PoseStamped()
         poseStampedTargetFrameChild.pose = posemath.toMsg(
             fTargetFrameChild)
         poseStampedTargetFrameChild.header.stamp = rospy.Time.now()
         poseStampedTargetFrameChild.header.frame_id = targetFrame
         return poseStampedTargetFrameChild
Esempio n. 26
0
    def set_pose(self, pos, rot, unit='rad', wait_callback=True):
        """

        :param pos_des: position array [x,y,z]
        :param rot_des: rotation array [Z,Y,X euler angle]
        :param unit: 'rad' or 'deg'
        :param wait_callback: True or False
        """
        if unit == 'deg':
            rot = U.deg_to_rad(rot)

        # limiting range of motion
        limit_angle = 89 * np.pi / 180
        if rot[1] > limit_angle: rot[1] = limit_angle
        elif rot[1] < -limit_angle: rot[1] = -limit_angle
        if rot[2] > limit_angle: rot[2] = limit_angle
        elif rot[2] < -limit_angle: rot[2] = -limit_angle

        # set in position cartesian mode
        frame = self.NumpyArraytoPyKDLFrame(pos, rot)
        msg = posemath.toMsg(frame)

        print msg
        # go to that position by goal
        if wait_callback:
            return self.__set_position_goal_cartesian_publish_and_wait(msg)
        else:
            self.goal_reached = False
            self.__set_position_goal_cartesian_pub.publish(msg)
            return True
Esempio n. 27
0
	def project_pose(self, pose):
		frame = pm.fromMsg(pose)
		[roll, pitch, yaw] = frame.M.GetRPY()
		frame.M = PyKDL.Rotation.RPY(0, 0, yaw)
		projected = pm.toMsg(frame)
		projected.position.z = 0
		return projected
Esempio n. 28
0
    def insert6DofGlobalMarker(self):
        int_position_marker = InteractiveMarker()
        int_position_marker.header.frame_id = 'world'
        int_position_marker.name = 'jar_marker'
        int_position_marker.scale = 0.2
        int_position_marker.pose = pm.toMsg(self.T_W_J)

        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'x'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'y'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'z'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'x'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'y'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'z'))

        box = self.createAxisMarkerControl(Point(0.15, 0.015, 0.015),
                                           Point(0.0, 0.0, 0.0))
        box.interaction_mode = InteractiveMarkerControl.BUTTON
        box.name = 'button'
        int_position_marker.controls.append(box)
        self.server.insert(int_position_marker, self.processFeedback)
        self.server.applyChanges()
def transformSE3(msg):
    global pose
    curPos = pm.Frame(
        pm.Rotation.Quaternion(msg.orientation.x, msg.orientation.y,
                               msg.orientation.z, msg.orientation.w),
        pm.Vector(msg.position.x, msg.position.y, msg.position.z))
    curVel = pm.Twist(
        pm.Vector(msg.linear_velocity.x, msg.linear_velocity.y,
                  msg.linear_velocity.z),
        pm.Vector(msg.angular_velocity.x, msg.angular_velocity.y,
                  msg.angular_velocity.z))
    pos = pose * curPos
    vel = pose * curVel
    tmp = pm.toMsg(pos)
    msg.position = tmp.position
    msg.orientation = tmp.orientation
    # Normalize orientation
    quat_original = np.array([
        msg.orientation.x, msg.orientation.y, msg.orientation.z,
        msg.orientation.w
    ])
    quat_normalized = quat_original / np.linalg.norm(quat_original)
    # Update orientation
    msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w = quat_normalized[
        0], quat_normalized[1], quat_normalized[2], quat_normalized[3]
    msg.linear_velocity = Vector3(vel.vel.x(), vel.vel.y(), vel.vel.z())
    msg.angular_velocity = Vector3(vel.rot.x(), vel.rot.y(), vel.rot.z())
Esempio n. 30
0
    def get_waypoints(self,frame_type,predicates,transforms,names):
        self.and_srv.wait_for_service()

        type_predicate = PredicateStatement()
        type_predicate.predicate = frame_type
        type_predicate.params = ['*','','']

        res = self.and_srv([type_predicate]+predicates)

        print "Found matches: " + str(res.matching)
        #print res

        if (not res.found) or len(res.matching) < 1:
          return None

        poses = []
        for tform in transforms:
            poses.append(pm.fromMsg(tform))

        #print poses
        new_poses = []
        new_names = []

        for match in res.matching:
            try:
                (trans,rot) = self.listener.lookupTransform(self.world,match,rospy.Time(0))
                for (pose, name) in zip(poses,names):
                    #resp.waypoints.poses.append(pm.toMsg(pose * pm.fromTf((trans,rot))))
                    new_poses.append(pm.toMsg(pm.fromTf((trans,rot)) * pose))
                    new_names.append(match + "/" + name)
            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                rospy.logwarn('Could not find transform from %s to %s!'%(self.world,match))
        
        return (new_poses, new_names)
Esempio n. 31
0
	def move_rel_to_cartesian_pose(self, time_from_start, rel_pose):
		print self.BCOLOR+"[IRPOS] Move relative to cartesian trajectory"+self.ENDC
				
		self.conmanSwitch([self.robot_name+'mPoseInt'], [], True)

		actual_pose = self.get_cartesian_pose()

		# Transform poses to frames.
		actualFrame = pm.fromMsg(actual_pose)
		
		relativeFrame = pm.fromMsg(rel_pose)
		
		desiredFrame = actualFrame * relativeFrame
		
		pose = pm.toMsg(desiredFrame)

		cartesianGoal = CartesianTrajectoryGoal()
		cartesianGoal.trajectory.points.append(CartesianTrajectoryPoint(rospy.Duration(time_from_start), pose, Twist()))
		cartesianGoal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)
		cartesianGoal.path_tolerance = CartesianTolerance(Vector3(self.CART_POS_TOLERANCE,self.CART_POS_TOLERANCE,self.CART_POS_TOLERANCE),Vector3(self.CART_ROT_TOLERANCE,self.CART_ROT_TOLERANCE,self.CART_ROT_TOLERANCE))
		cartesianGoal.goal_tolerance = CartesianTolerance(Vector3(self.CART_POS_TOLERANCE,self.CART_POS_TOLERANCE,self.CART_POS_TOLERANCE),Vector3(self.CART_ROT_TOLERANCE,self.CART_ROT_TOLERANCE,self.CART_ROT_TOLERANCE))
		
		
		self.pose_client.send_goal(cartesianGoal)
		self.pose_client.wait_for_result()

		result = self.pose_client.get_result()
		code = self.cartesian_error_code_to_string(result.error_code)
		print self.BCOLOR+"[IRPOS] Result: "+str(code)+self.ENDC

		self.conmanSwitch([], [self.robot_name+'mPoseInt'], True)
		return result
 def get_new_waypoint(self,obj):
     try:
         (trans,rot) = self.listener.lookupTransform(obj,self.endpoint,rospy.Time(0))
         return pm.toMsg(pm.fromTf((trans,rot)))
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         rospy.logerr('Failed to lookup transform from %s to %s'%(obj,self.endpoint))
     return None
Esempio n. 33
0
def LoopPosearrayInitpose(transformstamped, posestampedarray_src,
                          posestampedarray_target):
    tmp = 1
    id_src = checkHeaderID(transformstamped.header.frame_id,
                           posestampedarray_src)
    id_target = checkHeaderID(transformstamped.child_frame_id,
                              posestampedarray_target)
    # if ()
    if not (id_src > 0 and id_target > 0):
        print(
            "========================================checkHeaderID==========================="
        )
        print(
            "transformstamped.header.frame_id = {} :  posestampedarray_src = {}"
            .format(transformstamped.header.frame_id, posestampedarray_src))
        print(
            "transformstamped.child_frame_id = {} :  posestampedarray_target = {}"
            .format(transformstamped.child_frame_id, posestampedarray_target))
        # assert(False)
        return None

    trans_r1_f1_findex = posemath.fromMsg(
        posestampedarray_src.poseArray[id_src].pose)
    trans_r1_r2_findex_findex = posemath.fromMsg(
        trans2pose(transformstamped.transform)).Inverse()
    trans_r1_r2_f1_finex = trans_r1_f1_findex * trans_r1_r2_findex_findex
    trans_r2_findex_f1 = posemath.fromMsg(
        posestampedarray_target.poseArray[id_target].pose).Inverse()
    trans_r1_r2_f1_f1 = trans_r1_r2_f1_finex * trans_r2_findex_f1

    return posemath.toMsg(trans_r1_r2_f1_f1)
Esempio n. 34
0
  def calc_relative_pose(self, x,y,z,roll=0,pitch=0,yew=0, in_tip_frame=True):
    _pose=self.endpoint_pose()
    ########################################
    #  set target position
    trans = PyKDL.Vector(x,y,z)
    rot = PyKDL.Rotation.RPY(roll,pitch,yew)
    f2 = PyKDL.Frame(rot, trans)

    if in_tip_frame:
      # endpoint's cartesian systems
      _pose=posemath.toMsg(posemath.fromMsg(_pose) * f2)
    else:
      # base's cartesian systems
      _pose=posemath.toMsg(f2 * posemath.fromMsg(_pose))

    return _pose
Esempio n. 35
0
def transform_wrist_frame(T_tool, ft, tool_x_offset=0.0):
    '''
    :param T_tool: desired  *_gripper_tool_frame pose w.r.t. some reference frame. Can be of type kdl.Frame or geometry_msgs.Pose.
    :param ft: bool. True if the arm has a force-torque sensor
    :param tool_x_offset: (double) offset in tool length
    :return: T_torso_wrist. geometry_msgs.Pose type. Wrist pose w.r.t. same ref frame as T_tool
    '''

    if ft:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.216 + tool_x_offset, 0.0, 0.0))

    else:
        T_wrist_tool = kdl.Frame(kdl.Rotation.Identity(), kdl.Vector(0.180 + tool_x_offset, 0.0, 0.0))

    if type(T_tool) is Pose:

        T_tool_ = posemath.fromMsg(T_tool)

    elif type(T_tool) is tuple and len(T_tool) is 2:
        T_tool_ = posemath.fromTf(T_tool)

    else:
        T_tool_ = T_tool


    T_wrist_ = T_tool_*T_wrist_tool.Inverse()

    T_wrist = posemath.toMsg(T_wrist_)

    return T_wrist
Esempio n. 36
0
    def callback_state(self, data):
        # If state is not being reset proceed otherwise skip callback
        if self.reset.isSet():
            if self.real_robot:
                # Convert Pose from relative to Map to relative to World frame
                f_r_in_map = posemath.fromMsg(data)
                f_r_in_world = self.world_to_map * f_r_in_map
                data = posemath.toMsg(f_r_in_world)

            x = data.position.x
            y = data.position.y

            orientation = PyKDL.Rotation.Quaternion(data.orientation.x,
                                                    data.orientation.y,
                                                    data.orientation.z,
                                                    data.orientation.w)

            euler_orientation = orientation.GetRPY()
            yaw = euler_orientation[2]

            # Append Pose to Path
            stamped_mir_pose = PoseStamped()
            stamped_mir_pose.pose = data
            stamped_mir_pose.header.stamp = rospy.Time.now()
            stamped_mir_pose.header.frame_id = self.path_frame
            self.mir_path.poses.append(stamped_mir_pose)
            self.mir_exec_path.publish(self.mir_path)

            # Update internal Pose variable
            self.mir_pose = copy.deepcopy([x, y, yaw])
        else:
            pass
Esempio n. 37
0
def get_ik_simple(service, goal, seed, joint_names, tip_frame, tool=None):
    validate_quat(goal.pose.orientation)
    
    # Transforms the goal due to the tip frame
    if tool:
        tool_in_tip = tfl.transformPose(tip_frame, tool)
        tool_in_tip_t = pm.fromMsg(tool_in_tip.pose)
        goal_t = pm.fromMsg(goal.pose)
        #goal.pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t)
        #goal = PoseStamped(header = goal.header, pose = pm.toMsg(tool_in_tip_t.Inverse() * goal_t))
        goal = PoseStamped(header = goal.header, pose = pm.toMsg(goal_t * tool_in_tip_t.Inverse()))

    req = PositionIKRequest()
    req.ik_link_name = tip_frame
    req.pose_stamped = goal
    req.ik_seed_state.joint_state.name = joint_names
    req.ik_seed_state.joint_state.position = seed

    error_code = 0
    for i in range(10):
        resp = service(req, rospy.Duration(10.0))
        if resp.error_code.val != 1:
            rospy.logerr("Error in IK: %d" % resp.error_code.val)
        else:
            return list(resp.solution.joint_state.position)
    return []
Esempio n. 38
0
    def insert6DofGlobalMarker(self):
        T_B_T = self.velma.getTf("B", "T" + self.prefix)
        int_position_marker = InteractiveMarker()
        int_position_marker.header.frame_id = 'torso_base'
        int_position_marker.name = self.prefix + '_arm_position_marker'
        int_position_marker.scale = 0.2
        int_position_marker.pose = pm.toMsg(T_B_T)

        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'x'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'y'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.ROTATE_AXIS, 'z'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'x'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'y'))
        int_position_marker.controls.append(
            self.createInteractiveMarkerControl6DOF(
                InteractiveMarkerControl.MOVE_AXIS, 'z'))

        box = self.createAxisMarkerControl(Point(0.15, 0.015, 0.015),
                                           Point(0.0, 0.0, 0.0))
        box.interaction_mode = InteractiveMarkerControl.BUTTON
        box.name = 'button'
        int_position_marker.controls.append(box)
        self.server.insert(int_position_marker, self.processFeedback)
        self.server.applyChanges()
Esempio n. 39
0
def virtualObjectLeftHand():
     print "Creating a virtual object attached to gripper..."
     # for more details refer to ROS docs for moveit_msgs/AttachedCollisionObject
     object1 = AttachedCollisionObject()
     object1.link_name = "left_HandGripLink"
     object1.object.header.frame_id = "left_HandGripLink"
     object1.object.id = "object1"
     object1_prim = SolidPrimitive()
     object1_prim.type = SolidPrimitive.CYLINDER
     object1_prim.dimensions=[None, None]    # set initial size of the list to 2
     object1_prim.dimensions[SolidPrimitive.CYLINDER_HEIGHT] = 0.23
     object1_prim.dimensions[SolidPrimitive.CYLINDER_RADIUS] = 0.06
     object1_pose = pm.toMsg(PyKDL.Frame(PyKDL.Rotation.RotY(math.pi/2)))
     object1.object.primitives.append(object1_prim)
     object1.object.primitive_poses.append(object1_pose)
     object1.object.operation = CollisionObject.ADD
     object1.touch_links = ['left_HandPalmLink',
         'left_HandFingerOneKnuckleOneLink',
         'left_HandFingerOneKnuckleTwoLink',
         'left_HandFingerOneKnuckleThreeLink',
         'left_HandFingerTwoKnuckleOneLink',
         'left_HandFingerTwoKnuckleTwoLink',
         'left_HandFingerTwoKnuckleThreeLink',
         'left_HandFingerThreeKnuckleTwoLink',
         'left_HandFingerThreeKnuckleThreeLink']
     return object1
Esempio n. 40
0
def execute_cb(goal):
  rospy.loginfo("Action server received goal")
  preempt_timeout = rospy.Duration(5.0)

  for i in range(1,5):
    rospy.loginfo('Detecting plug in gripper from position %i'%i)

    # move to joint space position
    rospy.loginfo("Move in joint space...")
    if joint_space_client.send_goal_and_wait(get_action_goal('pr2_plugs_configuration/detect_plug_in_gripper%i'%i), rospy.Duration(20.0), preempt_timeout) != GoalStatus.SUCCEEDED:
      rospy.logerr('Move retract in joint space failed')
      server.set_aborted()
      return

    # call vision plug detection
    rospy.loginfo("Detecting plug...")
    detect_plug_goal = VisionPlugDetectionGoal()
    detect_plug_goal.camera_name = "/r_forearm_cam"
    #detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(pi/2, 0.0, -pi/9), PyKDL.Vector(-.03, 0, 0)).Inverse())
    detect_plug_goal.prior.pose = toMsg(PyKDL.Frame(PyKDL.Rotation.RPY(-pi/2, pi/9, 0.0), PyKDL.Vector(0.03, 0, -0.01)))
    detect_plug_goal.prior.header.stamp = rospy.Time.now()
    detect_plug_goal.prior.header.frame_id = "r_gripper_tool_frame"
    detect_plug_goal.origin_on_right = True
    detect_plug_goal.prior.header.stamp = rospy.Time.now()
    if detect_plug_client.send_goal_and_wait(detect_plug_goal, rospy.Duration(5.0), preempt_timeout) == GoalStatus.SUCCEEDED:
      server.set_succeeded(DetectPlugInGripperResult(detect_plug_client.get_result().plug_pose))      
      rospy.loginfo("Action server goal finished")  
      return

  # Failure
  rospy.logerr("Failed to detect plug in gripper")      
  server.set_aborted(DetectPlugInGripperResult(detect_plug_goal.prior))      
Esempio n. 41
0
	def move_rel_to_cartesian_pose_with_contact(self, time_from_start, rel_pose, wrench_constraint):
		print self.BCOLOR+"[IRPOS] Move relative to cartesian trajectory with contact"+self.ENDC
				
		self.conmanSwitch([self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], [], True)
		time.sleep(0.05)

		actual_pose = self.get_cartesian_pose()
		
		# Transform poses to frames.
		actualFrame = pm.fromMsg(actual_pose)
		
		relativeFrame = pm.fromMsg(rel_pose)
		
		desiredFrame = actualFrame * relativeFrame
		
		pose = pm.toMsg(desiredFrame)

		cartesianGoal = CartesianTrajectoryGoal()
		cartesianGoal.wrench_constraint = wrench_constraint
		cartesianGoal.trajectory.points.append(CartesianTrajectoryPoint(rospy.Duration(time_from_start), pose, Twist()))
		cartesianGoal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.1)

		self.pose_client.send_goal(cartesianGoal)
		self.pose_client.wait_for_result()

		result = self.pose_client.get_result()
		code = self.cartesian_error_code_to_string(result.error_code)
		print self.BCOLOR+"[IRPOS] Result: "+str(code)+self.ENDC

		self.conmanSwitch([], [self.robot_name+'mPoseInt', self.robot_name+'mForceTransformation'], True)
Esempio n. 42
0
    def __frame_to_tfxPose(self, frame):
        """ Function converts a PyKDL Frame object to a tfx object """
        """ We convert a PyKDL.Frame object to a ROS posemath object and then reconvert it to a tfx.canonical.CanonicalTransform object """

        """:returns: tfx pose """
        rosMsg = posemath.toMsg(frame)
        return tfx.pose(rosMsg) 
def publish_tf_to_tree(tf_msg, var_frame_id, var_child_frame_id):
    global br, listener
    print("start publish tf")
    listener.waitForTransform(var_frame_id, tf_msg.header.frame_id,
                              rospy.Time(), rospy.Duration(4.0))
    listener.waitForTransform(tf_msg.child_frame_id, var_child_frame_id,
                              rospy.Time(), rospy.Duration(4.0))
    frame_tf_msg = listener._buffer.lookup_transform(
        strip_leading_slash(var_frame_id),
        strip_leading_slash(tf_msg.header.frame_id), rospy.Time(0))
    child_frame_tf_msg = listener._buffer.lookup_transform(
        strip_leading_slash(tf_msg.child_frame_id),
        strip_leading_slash(var_child_frame_id), rospy.Time(0))
    print("frame_tf_msg:" + str(frame_tf_msg))
    print("child_frame_tf_msg:" + str(child_frame_tf_msg))
    frame_tf = posemath.fromMsg(trackutils.trans2pose(frame_tf_msg.transform))
    print("frame_tf:" + str(frame_tf))
    child_frame_tf = posemath.fromMsg(
        trackutils.trans2pose(child_frame_tf_msg.transform))
    print("child_frame_tf:" + str(child_frame_tf))
    tf_kdl = posemath.fromMsg(trackutils.trans2pose(tf_msg.transform))
    print("tf_kdl:" + str(tf_kdl))
    tf_msg.transform = trackutils.pose2trans(
        posemath.toMsg(frame_tf * tf_kdl * child_frame_tf))
    tf_msg.header.stamp = tf_msg.header.stamp
    tf_msg.header.frame_id = var_frame_id
    tf_msg.child_frame_id = var_child_frame_id
    print("publish:" + str(tf_msg))
    br.sendTransformMessage(tf_msg)
def maintain_tf_tree(odom_to_scene_before=None, odom_to_scene_after=None):
    global br, listener, self_ID
    base_to_scene = TransformStamped()
    base_to_scene.transform = trackutils.pose2trans(
        posemath.toMsg(
            kdl.Frame(kdl.Rotation.Quaternion(x=0.5, y=-0.5, z=0.5, w=-0.5))))
    base_to_scene.header.stamp = rospy.Time.now()
    base_to_scene.header.frame_id = "base_link{}".format(self_ID)
    base_to_scene.child_frame_id = "scene{}".format(self_ID)
    br.sendTransformMessage(base_to_scene)

    if odom_to_scene_before == None or odom_to_scene_after == None:
        map_to_odom = TransformStamped()
        map_to_odom.header.stamp = rospy.Time.now()
        map_to_odom.transform.rotation.w = 1.
        map_to_odom.header.frame_id = "/map{}".format(self_ID)
        map_to_odom.child_frame_id = "/odom{}".format(self_ID)
        print(map_to_odom)
        br.sendTransformMessage(map_to_odom)  #发送map和odom的TF树
    elif odom_to_scene_before == odom_to_scene_after:
        listener.waitForTransform("map{}".format(self_ID),
                                  "odom{}".format(self_ID), rospy.Time(),
                                  rospy.Duration(4.0))
        map_to_odom = listener._buffer.lookup_transform(
            "map{}".format(self_ID), "odom{}".format(self_ID), rospy.Time(0))
        map_to_odom.header.stamp = rospy.Time.now()
        br.sendTransformMessage(map_to_odom)  #发送map和odom的TF树
    else:
        listener.waitForTransform("map{}".format(self_ID),
                                  "odom{}".format(self_ID), rospy.Time(),
                                  rospy.Duration(4.0))
        map_to_odom = listener._buffer.lookup_transform(
            "map{}".format(self_ID), "odom{}".format(self_ID), rospy.Time(0))
        map_to_odom.header.stamp = rospy.Time.now()
        map_to_odom_kdl = posemath.fromMsg(
            trackutils.trans2pose(map_to_odom.transform))
        init_pose = kdl.Frame(
            kdl.Rotation.Quaternion(x=0.5, y=-0.5, z=0.5, w=-0.5))
        odom_to_scene_after_kdl = init_pose * posemath.fromMsg(
            odom_to_scene_after)
        odom_to_scene_before_kdl = init_pose * posemath.fromMsg(
            odom_to_scene_before)
        map_to_odom_kdl = map_to_odom_kdl * odom_to_scene_before_kdl * odom_to_scene_after_kdl.Inverse(
        )
        map_to_odom.transform = trackutils.pose2trans(
            posemath.toMsg(map_to_odom_kdl))
        br.sendTransformMessage(map_to_odom)  #发送map和odom的TF树
Esempio n. 45
0
    def get_desired_cartesian_position(self):
        """Get the :ref:`desired cartesian position <currentvdesired>` of the robot in terms of caretsian space.

        :returns: the desired position of the robot in cartesian space
        :rtype: `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_"""
        frame = self.__position_cartesian_desired
        tfx_pose = tfx.pose(posemath.toMsg(frame))
        return tfx_pose
Esempio n. 46
0
 def forward_kinematics_cb(self, req):
     ''' Run forward kinematics on a joint space pose and return the result.
     '''
     q = req.joint_state.position
     kdl_pose = pm.fromMatrix(self.kdl_kin.forward(q))
     pose_msg = pm.toMsg(kdl_pose)
     response = ForwardKinematicsResponse(pose=pose_msg, ack="SUCCESS")
     return response
 def get_world_pose(self):
     self.__call__()
     self.listener.waitForTransform("/world", self.object_name,
                                    rospy.Time(0), rospy.Duration(10))
     return pm.toMsg(
         pm.fromTf(
             self.listener.lookupTransform("/world", self.object_name,
                                           rospy.Time(0))))
def moveTool(tool_frame, t):
    global action_tool_client
    tool_pose = pm.toMsg(tool_frame)
    action_tool_goal = CartesianTrajectoryGoal()
    action_tool_goal.trajectory.header.stamp = rospy.Time.now()
    action_tool_goal.trajectory.points.append(CartesianTrajectoryPoint(
    rospy.Duration(t),
    tool_pose,
    Twist()))
    action_tool_client.send_goal(action_tool_goal)
Esempio n. 49
0
    def handle_rgbd(self, qq):
        if self.triggermode==True and self.find==False:
            return
        (imgmsg, caminfo, pointcloud) = qq
        if not pointcloud or pointcloud._type != 'sensor_msgs/PointCloud2':
            return
        print "rgbd"
        img = CvBridge().imgmsg_to_cv(imgmsg, "mono8")
        pcm = image_geometry.PinholeCameraModel()
        pcm.fromCameraInfo(caminfo)

        self.track(CvBridge().imgmsg_to_cv(imgmsg, "rgb8"))

        for (code, corners) in self.tracking.items():
                cx = 0.0
                cy = 0.0
                count = 0.0
                for (x,y) in corners:
                        cx += x
                        cy += y
                        count += 1.0
                cx /= count
                cy /= count      


                model = cv.fromarray(numpy.array([[0,0,0], [.1, 0, 0], [.1, .1, 0], [0, .1, 0]], numpy.float32))

                rot = cv.CreateMat(3, 1, cv.CV_32FC1)
                trans = cv.CreateMat(3, 1, cv.CV_32FC1)
                cv.FindExtrinsicCameraParams2(model,
                                              cv.fromarray(numpy.array(corners, numpy.float32)),
                                              pcm.intrinsicMatrix(),
                                              pcm.distortionCoeffs(),
                                              rot,
                                              trans)  

                #ts = geometry_msgs.msg.TransformStamped()
                #ts.header.frame_id = imgmsg.header.frame_id
                #ts.header.stamp = imgmsg.header.stamp
                #ts.child_frame_id = code
                #rot = cv.fromarray(rot)
                #trans = cv.fromarray(trans)
                posemsg = pm.toMsg(self.fromCameraParams(cv, rot, trans))
                points = read_points(pointcloud, None, False, [(int(cx), int(cy))])
                #pointcloud.data(cy*pointcloud.row_step+cx*pointcloud.point_step)
                print type(points)
                for point in points:
                    print type(point)
                    print point
                    posemsg.position = point
                #ts.transform.rotation = posemsg.orientation
                #print pcm.fullProjectionMatrix()
                #posemsg.position = points[0]
                print "Pose: ", posemsg.position
                self.broadcast(imgmsg.header, code, posemsg)
  def getposecb(self, a, b):
    print "Setting Pose based on world model"

    if((rospy.Time.now().to_sec() - b.pose.header.stamp.to_sec()) > 4.0):
      print "time now = ", rospy.Time.now().to_sec(), "time from pose = ", b.pose.header.stamp.to_sec()
      print "Time stamp of detection to old: diff=", (rospy.Time.now().to_sec() - b.pose.header.stamp.to_sec())
      if(self.detection_counter <= 4):
        self.detection_counter += 1
        return "preempted"
      else:
        self.detection_counter = 0
        return "aborted"

    self.config_over_object.header.stamp = rospy.Time.now()
    self.config_over_object.pose.position.x = b.pose.pose.position.x
    self.config_over_object.pose.position.y = b.pose.pose.position.y
    #ao = PyKDL.Rotation.Quaternion(self.config_over_object.pose.orientation.x, self.config_over_object.pose.orientation.y, self.config_over_object.pose.orientation.z, self.config_over_object.pose.orientation.w)
    #ao = PyKDL.Rotation.Quaternion(self.config_over_object.pose.orientation.x, self.config_over_object.pose.orientation.y, self.config_over_object.pose.orientation.z, self.config_over_object.pose.orientation.w)
    #bo = PyKDL.Rotation.Quaternion(b.pose.pose.orientation.x, b.pose.pose.orientation.y, b.pose.pose.orientation.z, b.pose.pose.orientation.w)
    #c = ao #* bo#.Inverse()
    m_manip = pm.toMatrix( pm.fromMsg(self.config_over_object.pose) )
    m_obj = pm.toMatrix( pm.fromMsg(b.pose.pose) )
    import numpy
    from tf.transformations import *
    m_ori = pm.toMatrix( pm.fromTf( ((0,0,0), quaternion_from_euler(0, 0, math.radians(-20.0))) ))
    box_pose = pm.toMsg( pm.fromMatrix(numpy.dot(m_manip,m_obj)) )
    m_test = numpy.dot(m_manip,m_obj)
    newrot = pm.toMsg( pm.fromMatrix(m_test))
    print newrot
    print self.config_over_object.pose
    #print c.GetQuaternion()
    self.config_over_object.pose.orientation = box_pose.orientation
    #self.config_over_object.pose.orientation.y = c.GetQuaternion()[1]
    #self.config_over_object.pose.orientation.z = c.GetQuaternion()[2]
    #self.config_over_object.pose.orientation.w = c.GetQuaternion()[3]
    #self.config_over_object.pose.position.z = 0.30#0.430

    self.config_object.header.stamp = rospy.Time.now()
    self.config_object.pose.position.x = b.pose.pose.position.x
    self.config_object.pose.position.y = b.pose.pose.position.y
    #self.config_object.pose.position.z = 0.2 #0.095
    self.config_object.pose.orientation = box_pose.orientation
    def call_planner(self, stage):
        self.status.planning_status = stage
        print(stage)

        if stage == ObjectStatus.PREGRASP:
            
            target_pose = self.status.pose_stamped.pose
            
            pregrasp_distance = .1

            #The marker pose is defined in the torso coordinate system
            target_tran = pm.toMatrix(pm.fromMsg(target_pose))
            hand_tran = target_tran.copy()


            #Align hand perpendicular to cylinder in its canonical pose
            hand_tran[:3,1] = target_tran[:3,2]
            hand_tran[:3,2] = target_tran[:3,0]
            hand_tran[:3,0] = target_tran[:3,1]
            pregrasp_tran = hand_tran.copy()

            #The approach direction of this gripper is -y for some reason
            pregrasp_tran[:3,3] = pregrasp_tran[:3,3] + pregrasp_tran[:3,1]*pregrasp_distance
            hand_tran_pose = pm.toMsg(pm.fromMatrix(hand_tran))
            pregrasp_pose = pm.toMsg(pm.fromMatrix(pregrasp_tran))


            req = PosePlanningSrv._request_class()
            req.TargetPoses = [pregrasp_pose, hand_tran_pose]
            req.ManipulatorID = int(self.status.hands == self.status.RIGHT)
            res = None
            try:
                print(req)
                res = self.pregrasp_planner_client.call(req)
                self.status.operating_status = self.status.PLANNING
                #print res
                self.last_plan = res.PlannedTrajectory
            except:
                res = None
                rospy.loginfo("Planner failed to pregrasp")
                         
                self.status.operating_status = self.status.ERROR
Esempio n. 52
0
    def __move_cartesian_direct(self, end_frame):
        """Move the arm to the end position by passing the trajectory generator.

        :param end_frame: the ending `PyKDL.Frame <http://docs.ros.org/diamondback/api/kdl/html/python/geometric_primitives.html>`_
        :returns: true if you had successfully move
        :rtype: Bool"""
        # set in position cartesian mode
        end_position = posemath.toMsg(end_frame)
        # go to that position directly
        self.__set_position_cartesian_pub.publish(end_position)
        return True
Esempio n. 53
0
 def publish_state(self, event):
   for joint, frame in self.frames.items():
     msg = PoseStamped()
     # Populate the msg
     msg.pose = posemath.toMsg(frame)
     # Publish the msg
     msg.header.frame_id = self.frame_id
     msg.header.stamp = rospy.Time.now()
     self.pose_pub[joint].publish(msg)
   # Draw the skeleton
   self.draw_robot()
Esempio n. 54
0
                def plug_in_contact(ud):
                    """Returns true if the plug is in contact with something."""

                    pose_base_gripper = fromMsg(TFUtil.wait_and_lookup('base_link', 'r_gripper_tool_frame').pose)
                    pose_outlet_plug = fromMsg(ud.base_to_outlet).Inverse() * pose_base_gripper * fromMsg(ud.gripper_to_plug)

                    # check if difference between desired and measured outlet-plug along x-axis is more than 1 cm
                    ud.outlet_to_plug_contact = toMsg(pose_outlet_plug)
                    if math.fabs(pose_outlet_plug.p[0] - ud.approach_offset) > 0.01:
                        return True
                    return False
Esempio n. 55
0
    def test_roundtrip(self):
        c = Frame()

        d = pm.fromMsg(pm.toMsg(c))
        self.assertEqual(repr(c), repr(d))

        d = pm.fromMatrix(pm.toMatrix(c))
        self.assertEqual(repr(c), repr(d))

        d = pm.fromTf(pm.toTf(c))
        self.assertEqual(repr(c), repr(d))
Esempio n. 56
0
    def handle_stereo(self, qq):

        (lmsg, lcmsg, rmsg, rcmsg) = qq
        limg = CvBridge().imgmsg_to_cv(lmsg, "mono8")
        rimg = CvBridge().imgmsg_to_cv(rmsg, "mono8")

        if 0:
            cv.SaveImage("/tmp/l%06d.png" % self.i, limg)
            cv.SaveImage("/tmp/r%06d.png" % self.i, rimg)
            self.i += 1

        scm = image_geometry.StereoCameraModel()
        scm.fromCameraInfo(lcmsg, rcmsg)

        bm = cv.CreateStereoBMState()
        if "wide" in rospy.resolve_name("stereo"):
            bm.numberOfDisparities = 160
        if 0:
            disparity = cv.CreateMat(limg.rows, limg.cols, cv.CV_16SC1)
            started = time.time()
            cv.FindStereoCorrespondenceBM(limg, rimg, disparity, bm)
            print time.time() - started
            ok = cv.CreateMat(limg.rows, limg.cols, cv.CV_8UC1)
            cv.CmpS(disparity, 0, ok, cv.CV_CMP_GT)
            cv.ShowImage("limg", limg)
            cv.ShowImage("disp", ok)
            cv.WaitKey(6)
        self.track(CvBridge().imgmsg_to_cv(lmsg, "rgb8"))
        if len(self.tracking) == 0:
            print "No markers found"
        for code, corners in self.tracking.items():
            corners3d = []
            for (x, y) in corners:
                limr = cv.GetSubRect(limg, (0, y - bm.SADWindowSize / 2, limg.cols, bm.SADWindowSize + 1))
                rimr = cv.GetSubRect(rimg, (0, y - bm.SADWindowSize / 2, rimg.cols, bm.SADWindowSize + 1))
                tiny_disparity = cv.CreateMat(limr.rows, limg.cols, cv.CV_16SC1)
                cv.FindStereoCorrespondenceBM(limr, rimr, tiny_disparity, bm)
                if tiny_disparity[7, x] < 0:
                    return
                corners3d.append(scm.projectPixelTo3d((x, y), tiny_disparity[7, x] / 16.0))
                if 0:
                    cv.ShowImage("d", disparity)
            (a, b, c, d) = [numpy.array(pt) for pt in corners3d]

            def normal(s, t):
                return (t - s) / numpy.linalg.norm(t - s)

            x = PyKDL.Vector(*normal(a, b))
            y = PyKDL.Vector(*normal(a, d))
            f = PyKDL.Frame(PyKDL.Rotation(x, y, x * y), PyKDL.Vector(*a))
            msg = pm.toMsg(f)
            # print "%10f %10f %10f" % (msg.position.x, msg.position.y, msg.position.z)
            print code, msg.position.x, msg.position.y, msg.position.z
            self.broadcast(lmsg.header, code, msg)
Esempio n. 57
0
    def __setstate__(self, state):
        self.model_name = state['model_name']
        self.object_name = state['object_name']

        buff = StringIO.StringIO()
        buff.writelines(state['point_cloud_data'])
        buff.seek(0)
        self.point_cloud_data = sensor_msgs.msg.PointCloud2()
        self.point_cloud_data.deserialize(buff.buf)
        self.pose = pm.toMsg(pm.fromMatrix(state['pose']))
        self.bc = ModelRecManager.tf_broadcaster
        self.listener = ModelRecManager.tf_listener
Esempio n. 58
0
    def joy_cb(self, msg):
        # Convenience
        side = self.side
        b = msg.buttons
        lb = self.last_buttons

        if (rospy.Time.now() - self.last_hand_cmd) < rospy.Duration(0.1):
            return

        # Update enabled fingers
        self.move_f[0] =   self.move_f[0] ^   (b[self.B1[side]] and not lb[self.B1[side]])
        self.move_f[1] =   self.move_f[1] ^   (b[self.B2[side]] and not lb[self.B2[side]])
        self.move_f[2] =   self.move_f[2] ^   (b[self.B3[side]] and not lb[self.B3[side]])
        self.move_spread = self.move_spread ^ (b[self.B4[side]] and not lb[self.B4[side]])
        self.move_all =    self.move_all ^    (b[self.B_CENTER[side]] and not lb[self.B_CENTER[side]])

        # Check if the deadman is engaged
        if msg.axes[self.DEADMAN[side]] < 0.01:
            if self.deadman_engaged:
                self.hand_cmd.mode = [oro_barrett_msgs.msg.BHandCmd.MODE_VELOCITY] * 4
                self.hand_cmd.cmd = [0.0, 0.0, 0.0, 0.0]
                self.hand_pub.publish(self.hand_cmd)
                self.last_hand_cmd = rospy.Time.now()
            self.deadman_engaged = False
            self.deadman_max = 0.0
        else:
            self.handle_hand_cmd(msg)
            self.handle_cart_cmd(msg)

        self.last_buttons = msg.buttons
        self.last_axes = msg.axes

        # Broadcast the command if it's defined
        if self.cmd_frame:
            # Broadcast command frame
            tform = toTf(self.cmd_frame)
            self.broadcaster.sendTransform(tform[0], tform[1], rospy.Time.now(), self.cmd_frame_id, 'world')

            # Broadcast telemanip command
            telemanip_cmd = TelemanipCommand()
            telemanip_cmd.header.frame_id = 'world'
            telemanip_cmd.header.stamp = rospy.Time.now()
            telemanip_cmd.posetwist.pose = toMsg(self.cmd_frame)
            telemanip_cmd.resync_pose = msg.buttons[self.THUMB_CLICK[side]] == 1
            telemanip_cmd.deadman_engaged = self.deadman_engaged
            if msg.axes[self.THUMB_Y[side]] > 0.5:
                telemanip_cmd.grasp_opening = 0.0
            elif msg.axes[self.THUMB_Y[side]] < -0.5:
                telemanip_cmd.grasp_opening = 1.0
            else:
                telemanip_cmd.grasp_opening = 0.5
            telemanip_cmd.estop = False  # TODO: add master estop control
            self.telemanip_cmd_pub.publish(telemanip_cmd)